智能车摄像头开源—6 边线信息提取

目次

一、媒介
二、直角拐点求取
1、根据生长方向找拐点
1.    左侧上转左拐点
2.    左侧右转上拐点
3.    右侧上转右拐点
4.    右侧左转上拐点
2、根据X坐标跳变寻找拐点
3、组合判断拐点
4、逆透视求角度找拐点
三、边界点求取
四、对位边界行
五、圆弧拐点求取
六、求每侧边线斜率及截距

七、拟合曲线计算方差
八、求取左右侧边线差值
九、TOF测距
十、总函数调用
十一、总结

---

一、媒介

        本文主要讲授二维数组边线的信息提取，此中主要包罗：直角拐点、圆弧拐点、边界点个数等。
二、直角拐点求取

1、根据生长方向找拐点

        岂论是常见的八邻域求边线，还是本人优化过的八向迷宫求取边线，都可以得到每个边线点的生长方向，由此我们可以判断直角拐点。
        因摄像头为斜视，因此获取图像中的直角拐点并非严酷的直角拐点，而是锐角，由此我们根据生长方向判断拐点时，
        此处基于本人的八向迷宫讲授拐点求取。八向迷宫的生长方向差别于八邻域的0~7，而是特别的数字用来代表（可以参考之前的文章：智能车摄像头开源—1.2 核心算法：自适应八向迷宫（下））：

1. -2, 1, 4
2. -3,    3
3. -4,-1, 2

复制代码

        直角拐点主要需求四类：上转左，上转右，左转上，右转上。这里的上、下、左、右为生长的大抵方向，比方上转左拐点，即边线大要上向上生长，经过上转左拐点后，转为大要上向左生长。此中，上转左和右转上只会在左侧边线出现，而上转右，左转上只会出现在右侧边线（二维边线）。当碰到十字路口时（正入十字，斜入另说），会碰到上述四个拐点，循环找四个拐点给标记位，即可初步判出十字元素。不可太过绝对。但同时不可太过简单，否则导致误判，在一定程度上，误判比漏判结果更为严峻。
        读者可求取一张十字的图像，因摄像头斜视原因，会发现向上生长的边线为斜线，向左或向右生长的边线较趋近于平滑的直线
        下基于八向迷宫的八个方向分别讲授四个拐点，每个拐点有三个判断严酷程度，每个拐点需根据前后几个点的生长方向加以判断。
1.    左侧上转左拐点

        宽松判断：前方几个点的生长方向为 -2 或 1 或 4 ，后方几个点生长方向为 -2 或 -3 或 -4；
        中等判断：前方几个点的生长方向为 1 或 4 ，后方几个点生长方向为 -3 或 -4；
        严酷判断：前方几个点的生长方向为 1 或 4 ，后方几个点生长方向为 -3 ；
        此中宽松判断会和圆弧拐点误判，中等判断常用，严酷判断要求过高，根据自身需求调用。

1. /**
2. * 函数功能：      寻找上转左拐点
3. * 特殊说明：      分为三个严格等级，根据每个点的生长方向判断
4. *                 寻找方式为遍历二维数组边线的每个点
5. * 形  参：        uint8 Grade      选择判定严格等级，常用2，即中等等级
6. *
7. * 示例：          Get_L_Up_Turn_Left_Point_1(2);
8. * 返回值：        无
9. */
10. uint8 L_Up_Turn_Left_Point_1[2] = {0};      //存储上转左拐点的坐标
11. uint8 L_Up_Turn_Left_Point_Flag_1 = 0;      //上转左拐点存在标志位，找到时置1
12. uint8 L_Up_Turn_Left_Point_Position_1 = 0;  //记录位置，即是二维数组中的第几个点
13. float L_Up_Turn_Left_Point_Angle_1 = 0;     //记录找到的拐点的角度（逆透视求取）
14. void Get_L_Up_Turn_Left_Point_1(uint8 Grade)
15. {
16.     uint8 i = 0;
17.     L_Up_Turn_Left_Point_Flag_1 = 0;
18.     L_Up_Turn_Left_Point_1[0] = 0;
19.     L_Up_Turn_Left_Point_1[1] = 0;
20.     L_Up_Turn_Left_Point_Position_1 = 0;
21.     L_Up_Turn_Left_Point_Angle_1 = 0;       //将各个参数清零
22.     switch(Grade)
23.     {
24.         case 1:     //严格判断
25.         {
26.             for (i = 4; i < (L_Statics - 4); i++)    //L_Statics 为左侧边线点个数
27.             {
28.                 if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))    //L_Line为存储左侧边线的二维数组，拐点必须满足不在于左右两侧边界上
29.                 {
30.                     if ((L_Grow_Dir[i - 2] == 4 || L_Grow_Dir[i - 2] == 1) && (L_Grow_Dir[i - 4] == 4 || L_Grow_Dir[i - 4] == 1) &&
31.                         (L_Grow_Dir[i + 2] == -3) && (L_Grow_Dir[i + 4] == -3) && (L_Grow_Dir[i] == -2 || L_Grow_Dir[i] == -3))    //L_Grow_Dir存储每个边线点的生长方向
32.                     {
33.                         L_Up_Turn_Left_Point_1[0] = L_Line[i][0];
34.                         L_Up_Turn_Left_Point_1[1] = L_Line[i][1];
35.                         L_Up_Turn_Left_Point_Flag_1  = 1;
36.                         break;
37.                     }
38.                 }
39.             }
40.             break;
41.         }
42.         case 2:     //中等判断
43.         {
44.             for (i = 4; i < (L_Statics - 4); i++)
45.             {
46.                 if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
47.                 {
48.                     if ((L_Grow_Dir[i - 2] == 4 || L_Grow_Dir[i - 2] == 1) && (L_Grow_Dir[i - 4] == 4 || L_Grow_Dir[i - 4] == 1) &&
49.                         (L_Grow_Dir[i + 2] == -3 || L_Grow_Dir[i + 2] == -4) && (L_Grow_Dir[i + 4] == -3 || L_Grow_Dir[i + 4] == -4) &&
50.                         (L_Grow_Dir[i] == -2 || L_Grow_Dir[i] == -3 || L_Grow_Dir[i] == -4))
51.                     {
52.                         L_Up_Turn_Left_Point_1[0] = L_Line[i][0];
53.                         L_Up_Turn_Left_Point_1[1] = L_Line[i][1];
54.                         L_Up_Turn_Left_Point_Flag_1  = 1;
55.                         break;
56.                     }
57.                 }
58.             }
59.             break;
60.         }
61.         case 3:     //宽松判断
62.         {
63.             for (i = 4; i < (L_Statics - 4); i++)
64.             {
65.                 if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
66.                 {
67.                     if ((L_Grow_Dir[i - 2] == 4 || L_Grow_Dir[i - 2] == 1 || L_Grow_Dir[i - 2] == -2) && (L_Grow_Dir[i - 4] == 4 || L_Grow_Dir[i - 4] == 1 || L_Grow_Dir[i - 2] == -2) &&
68.                         (L_Grow_Dir[i + 2] == -3 || L_Grow_Dir[i + 2] == -4 || L_Grow_Dir[i + 2] == -2) && (L_Grow_Dir[i + 4] == -3 || L_Grow_Dir[i + 4] == -4 || L_Grow_Dir[i + 4] == -2) &&
69.                         (L_Grow_Dir[i] == -2 || L_Grow_Dir[i] == -3 || L_Grow_Dir[i] == -4))
70.                     {
71.                         L_Up_Turn_Left_Point_1[0] = L_Line[i][0];
72.                         L_Up_Turn_Left_Point_1[1] = L_Line[i][1];
73.                         L_Up_Turn_Left_Point_Flag_1  = 1;
74.                         break;
75.                     }
76.                 }
77.             }
78.             break;
79.         }
80.     }
81.     if(L_Up_Turn_Left_Point_Flag_1 == 1)
82.     {
83.         L_Up_Turn_Left_Point_Position_1 = i;    //记录拐点的位置
84.         L_Up_Turn_Left_Point_Angle_1 = Get_Turn_Point_Angle(L_Line[i - 5][0], L_Line[i - 5][1], L_Line[i][0], L_Line[i][1], L_Line[i + 5][0], L_Line[i + 5][1]);    //逆透视求取拐点角度，后续补充
85.     }
86. }

复制代码

2.    左侧右转上拐点

        宽松判断：前方几个点的生长方向为 2 或 3 或 4 ，后方几个点生长方向为 4 或 1 或 -2；
        中等判断：前方几个点的生长方向为 2或 3 ，后方几个点生长方向为 4 或 1；
        严酷判断：前方几个点的生长方向为 3 ，后方几个点生长方向为 4 或 1 ；
        与左侧上转左拐点同理，下面几个都不做过多讲述。

1. /**
2. * 函数功能：      寻找右转上拐点
3. * 特殊说明：      分为三个严格等级，根据每个点的生长方向判断
4. *                 寻找方式为遍历二维数组边线的每个点
5. * 形  参：        uint8 Grade      选择判定严格等级，常用2，即中等等级
6. *
7. * 示例：          Get_L_Right_Turn_Up_Point_1(2);
8. * 返回值：        无
9. */
10. uint8 L_Right_Turn_Up_Point_1[2] = {0};
11. uint8 L_Right_Turn_Up_Point_Flag_1 = 0;
12. uint8 L_Right_Turn_Up_Point_Position_1 = 0;
13. float L_Right_Turn_Up_Point_Angle_1 = 0;
14. void Get_L_Right_Turn_Up_Point_1(uint8 Grade)
15. {
16.     uint8 i = 0;
17.     L_Right_Turn_Up_Point_Flag_1 = 0;
18.     L_Right_Turn_Up_Point_1[0] = 0;
19.     L_Right_Turn_Up_Point_1[1] = 0;
20.     L_Right_Turn_Up_Point_Position_1 = 0;
21.     L_Right_Turn_Up_Point_Angle_1 = 0;
22.     switch(Grade)
23.     {
24.         case 1:
25.         {
26.             for (i = 5; i < (L_Statics - 4); i++)
27.             {
28.                 if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
29.                 {
30.                     if ((L_Grow_Dir[i + 2] == 4 || L_Grow_Dir[i + 2] == 1) && (L_Grow_Dir[i + 5] == 4 || L_Grow_Dir[i + 5] == 1) && (L_Grow_Dir[i + 7] == 4 || L_Grow_Dir[i + 7] == 1) &&
31.                         (L_Grow_Dir[i - 2] == 3) && (L_Grow_Dir[i - 5] == 3) && (L_Grow_Dir[i] == 4 || L_Grow_Dir[i] == 1))
32.                     {
33.                         L_Right_Turn_Up_Point_1[0] = L_Line[i][0];
34.                         L_Right_Turn_Up_Point_1[1] = L_Line[i][1];
35.                         L_Right_Turn_Up_Point_Flag_1  = 1;
36.                         break;
37.                     }
38.                 }
39.             }
40.             break;
41.         }
42.         case 2:
43.         {
44.             for (i = 5; i < (L_Statics - 4); i++)
45.             {
46.                 if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
47.                 {
48.                     if ((L_Grow_Dir[i + 2] == 4 || L_Grow_Dir[i + 2] == 1) && (L_Grow_Dir[i + 5] == 4 || L_Grow_Dir[i + 5] == 1) && (L_Grow_Dir[i + 7] == 4 || L_Grow_Dir[i + 7] == 1) &&
49.                         (L_Grow_Dir[i - 2] == 2 || L_Grow_Dir[i - 2] == 3) && (L_Grow_Dir[i - 5] == 2 || L_Grow_Dir[i - 5] == 3) &&
50.                         (L_Grow_Dir[i] == 4 || L_Grow_Dir[i] == 1))
51.                     {
52.                         L_Right_Turn_Up_Point_1[0] = L_Line[i][0];
53.                         L_Right_Turn_Up_Point_1[1] = L_Line[i][1];
54.                         L_Right_Turn_Up_Point_Flag_1  = 1;
55.                         break;
56.                     }
57.                 }
58.             }
59.             break;
60.         }
61.         case 3:
62.         {
63.             for (i = 5; i < (L_Statics - 4); i++)
64.             {
65.                 if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
66.                 {
67.                     if ((L_Grow_Dir[i + 2] == 4 || L_Grow_Dir[i + 2] == 1 || L_Grow_Dir[i + 2] == -2) && (L_Grow_Dir[i + 5] == 4 || L_Grow_Dir[i + 5] == 1 || L_Grow_Dir[i + 5] == -2) &&
68.                         (L_Grow_Dir[i + 7] == 4 || L_Grow_Dir[i + 7] == 1 || L_Grow_Dir[i + 7] == -2) &&
69.                         (L_Grow_Dir[i - 2] == 2 || L_Grow_Dir[i - 2] == 3 || L_Grow_Dir[i - 2] == 4) && (L_Grow_Dir[i - 5] == 2 || L_Grow_Dir[i - 5] == 3 || L_Grow_Dir[i - 5] == 4) &&
70.                         (L_Grow_Dir[i] == 4 || L_Grow_Dir[i] == 1))
71.                     {
72.                         L_Right_Turn_Up_Point_1[0] = L_Line[i][0];
73.                         L_Right_Turn_Up_Point_1[1] = L_Line[i][1];
74.                         L_Right_Turn_Up_Point_Flag_1  = 1;
75.                         break;
76.                     }
77.                 }
78.             }
79.             break;
80.         }
81.     }
82.     if(L_Right_Turn_Up_Point_Flag_1 == 1)
83.     {
84.         L_Right_Turn_Up_Point_Position_1 = i;
85.         L_Right_Turn_Up_Point_Angle_1 = Get_Turn_Point_Angle(L_Line[i - 5][0], L_Line[i - 5][1], L_Line[i][0], L_Line[i][1], L_Line[i + 5][0], L_Line[i + 5][1]);
86.     }
87. }

复制代码

3.    右侧上转右拐点

        宽松判断：前方几个点的生长方向为 -2 或 1或 4 ，后方几个点生长方向为 2或 3或 4；
        中等判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 2或 3；
        严酷判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 3 ；

1. /**
2. * 函数功能：      寻找上转右拐点
3. * 特殊说明：      分为三个严格等级，根据每个点的生长方向判断
4. *                 寻找方式为遍历二维数组边线的每个点
5. * 形  参：        uint8 Grade      选择判定严格等级，常用2，即中等等级
6. *
7. * 示例：          Get_R_Up_Turn_Right_Point_1(2);
8. * 返回值：        无
9. */
10. uint8 R_Up_Turn_Right_Point_1[2] = {0};
11. uint8 R_Up_Turn_Right_Point_Flag_1 = 0;
12. uint8 R_Up_Turn_Right_Point_Position_1 = 0;
13. float R_Up_Turn_Right_Point_Angle_1 = 0;
14. void Get_R_Up_Turn_Right_Point_1(uint8 Grade)
15. {
16.     uint8 i = 0;
17.     R_Up_Turn_Right_Point_Flag_1 = 0;
18.     R_Up_Turn_Right_Point_1[0] = 0;
19.     R_Up_Turn_Right_Point_1[1] = 0;
20.     R_Up_Turn_Right_Point_Position_1 = 0;
21.     R_Up_Turn_Right_Point_Angle_1 = 0;
23.     switch(Grade)
24.     {
25.         case 1:
26.         {
27.             for (i = 4; i < (R_Statics - 4); i++)
28.             {
29.                 if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
30.                 {
31.                     if ((R_Grow_Dir[i - 2] == -2 || R_Grow_Dir[i - 2] == 1) && (R_Grow_Dir[i - 4] == -2 || R_Grow_Dir[i - 4] == 1) &&
32.                         (R_Grow_Dir[i + 2] == 3) && (R_Grow_Dir[i + 4] == 3) && (R_Grow_Dir[i] == 2 || R_Grow_Dir[i] == 3 || R_Grow_Dir[i] == 4))
33.                     {
34.                         R_Up_Turn_Right_Point_1[0] = R_Line[i][0];
35.                         R_Up_Turn_Right_Point_1[1] = R_Line[i][1];
36.                         R_Up_Turn_Right_Point_Flag_1 = 1;
37.                         break;
38.                     }
39.                 }
40.             }
41.             break;
42.         }
43.         case 2:
44.         {
45.             for (i = 4; i < (R_Statics - 4); i++)
46.             {
47.                 if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
48.                 {
49.                     if ((R_Grow_Dir[i - 2] == -2 || R_Grow_Dir[i - 2] == 1) && (R_Grow_Dir[i - 4] == -2 || R_Grow_Dir[i - 4] == 1) &&
50.                         (R_Grow_Dir[i + 2] == 2 || R_Grow_Dir[i + 2] == 3) && (R_Grow_Dir[i + 4] == 2 || R_Grow_Dir[i + 4] == 3) &&
51.                         (R_Grow_Dir[i] == 2 || R_Grow_Dir[i] == 3 || R_Grow_Dir[i] == 4))
52.                     {
53.                         R_Up_Turn_Right_Point_1[0] = R_Line[i][0];
54.                         R_Up_Turn_Right_Point_1[1] = R_Line[i][1];
55.                         R_Up_Turn_Right_Point_Flag_1 = 1;
56.                         break;
57.                     }
58.                 }
59.             }
60.             break;
61.         }
62.         case 3:
63.         {
64.             for (i = 4; i < (R_Statics - 4); i++)
65.             {
66.                 if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
67.                 {
68.                     if ((R_Grow_Dir[i - 2] == -2 || R_Grow_Dir[i - 2] == 1 || R_Grow_Dir[i - 2] == 4) && (R_Grow_Dir[i - 4] == -2 || R_Grow_Dir[i - 4] == 1 || R_Grow_Dir[i - 4] == 4) &&
69.                         (R_Grow_Dir[i + 2] == 2 || R_Grow_Dir[i + 2] == 3 || R_Grow_Dir[i + 2] == 4) && (R_Grow_Dir[i + 4] == 2 || R_Grow_Dir[i + 4] == 3 || R_Grow_Dir[i + 4] == 4) &&
70.                         (R_Grow_Dir[i] == 2 || R_Grow_Dir[i] == 3 || R_Grow_Dir[i] == 4))
71.                     {
72.                         R_Up_Turn_Right_Point_1[0] = R_Line[i][0];
73.                         R_Up_Turn_Right_Point_1[1] = R_Line[i][1];
74.                         R_Up_Turn_Right_Point_Flag_1 = 1;
75.                         break;
76.                     }
77.                 }
78.             }
79.             break;
80.         }
81.     }
82.     if(R_Up_Turn_Right_Point_Flag_1 == 1)
83.     {
84.         R_Up_Turn_Right_Point_Position_1 = i;
85.         R_Up_Turn_Right_Point_Angle_1 = Get_Turn_Point_Angle(R_Line[i - 5][0], R_Line[i - 5][1], R_Line[i][0], R_Line[i][1], R_Line[i + 5][0], R_Line[i + 5][1]);
86.     }
87. }

复制代码

4.    右侧左转上拐点

        宽松判断：前方几个点的生长方向为 -2 或 1或 4 ，后方几个点生长方向为 2或 3或 4；
        中等判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 2或 3；
        严酷判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 3 ；

1. /**
2. * 函数功能：      寻找左转上拐点
3. * 特殊说明：      分为三个严格等级，根据每个点的生长方向判断
4. *                 寻找方式为遍历二维数组边线的每个点
5. * 形  参：        uint8 Grade      选择判定严格等级，常用2，即中等等级
6. *
7. * 示例：          Get_R_Left_Turn_Up_Point_1(2);
8. * 返回值：        无
9. */
10. uint8 R_Left_Turn_Up_Point_1[2] = {0};
11. uint8 R_Left_Turn_Up_Point_Flag_1 = 0;
12. uint8 R_Left_Turn_Up_Point_Position_1 = 0;
13. float R_Left_Turn_Up_Point_Angle_1 = 0;
14. void Get_R_Left_Turn_Up_Point_1(uint8 Grade)
15. {
16.     uint8 i =0;
17.     R_Left_Turn_Up_Point_Flag_1 = 0;
18.     R_Left_Turn_Up_Point_1[0] = 0;
19.     R_Left_Turn_Up_Point_1[1] = 0;
20.     R_Left_Turn_Up_Point_Position_1 = 0;
21.     R_Left_Turn_Up_Point_Angle_1 = 0;
22.     switch(Grade)
23.     {
24.         case 1:
25.         {
26.             for (i = 5; i < (R_Statics - 4); i++)
27.             {
28.                 if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
29.                 {
30.                     if ((R_Grow_Dir[i + 2] == -2 || R_Grow_Dir[i + 2] == 1) && (R_Grow_Dir[i + 5] == -2 || R_Grow_Dir[i + 5] == 1) && (R_Grow_Dir[i + 7] == -2 || R_Grow_Dir[i + 7] == 1) &&
31.                         (R_Grow_Dir[i - 2] == -3) && (R_Grow_Dir[i - 5] == -3) && (R_Grow_Dir[i] == -2 || R_Grow_Dir[i] == 1))
32.                     {
33.                         R_Left_Turn_Up_Point_1[0] = R_Line[i][0];
34.                         R_Left_Turn_Up_Point_1[1] = R_Line[i][1];
35.                         R_Left_Turn_Up_Point_Flag_1 = 1;
36.                         break;
37.                     }
38.                 }
39.             }
40.             break;
41.         }
42.         case 2:
43.         {
44.             for (i = 5; i < (R_Statics - 4); i++)
45.             {
46.                 if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
47.                 {
48.                     if ((R_Grow_Dir[i + 2] == -2 || R_Grow_Dir[i + 2] == 1) && (R_Grow_Dir[i + 5] == -2 || R_Grow_Dir[i + 5] == 1) && (R_Grow_Dir[i + 7] == -2 || R_Grow_Dir[i + 7] == 1) &&
49.                         (R_Grow_Dir[i - 2] == -3 || R_Grow_Dir[i - 2] == -4) && (R_Grow_Dir[i - 5] == -3 || R_Grow_Dir[i - 5] == -4) && (R_Grow_Dir[i] == -2 || R_Grow_Dir[i] == 1))
50.                     {
51.                         R_Left_Turn_Up_Point_1[0] = R_Line[i][0];
52.                         R_Left_Turn_Up_Point_1[1] = R_Line[i][1];
53.                         R_Left_Turn_Up_Point_Flag_1 = 1;;
54.                         break;
55.                     }
56.                 }
57.             }
58.             break;
59.         }
60.         case 3:
61.         {
62.             for (i = 5; i < (R_Statics - 4); i++)
63.             {
64.                 if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
65.                 {
66.                     if ((R_Grow_Dir[i + 2] == -2 || R_Grow_Dir[i + 2] == 1 || R_Grow_Dir[i + 2] == 4) && (R_Grow_Dir[i + 5] == -2 || R_Grow_Dir[i + 5] == 1 || R_Grow_Dir[i + 5] == 4) &&
67.                         (R_Grow_Dir[i + 7] == -2 || R_Grow_Dir[i + 7] == 1 || R_Grow_Dir[i + 7] == 4) &&
68.                         (R_Grow_Dir[i - 2] == -3 || R_Grow_Dir[i - 2] == -4) && (R_Grow_Dir[i - 5] == -3 || R_Grow_Dir[i - 5] == -4) && (R_Grow_Dir[i] == -2 || R_Grow_Dir[i] == 1))
69.                     {
70.                         R_Left_Turn_Up_Point_1[0] = R_Line[i][0];
71.                         R_Left_Turn_Up_Point_1[1] = R_Line[i][1];
72.                         R_Left_Turn_Up_Point_Flag_1 = 1;;
73.                         break;
74.                     }
75.                 }
76.             }
77.             break;
78.         }
79.     }
80.     if(R_Left_Turn_Up_Point_Flag_1 == 1)
81.     {
82.         R_Left_Turn_Up_Point_Position_1 = i;
83.         R_Left_Turn_Up_Point_Angle_1 = Get_Turn_Point_Angle(R_Line[i - 5][0], R_Line[i - 5][1], R_Line[i][0], R_Line[i][1], R_Line[i + 5][0], R_Line[i + 5][1]);
84.     }
85. }

复制代码

2、根据X坐标跳变寻找拐点

        X坐标跳变寻拐点，使用的是一维边线数组。对于一维边线数组来说，每行左右边线只会存储一个点，即每行只会有两个边线点，差别于二维数组每行图像可以记载多个点。当碰到十字路口时对比尤为明显。
        比方在十字路口时，正常的二维数组应是如下环境：

        但一维数组存储的边线，应是如下环境：（画成蓝线更明显）

        可以对比上下两张图像，一维边线失去了横向的多个点，即每行每个边线只存储一个点，由此在四个拐点处，会出现当前拐点的X值与下一个点的（或前一个点的）X值出现较大变革，通过遍历每侧的一维边线每个点，可以找到四个拐点。同时需判断其与其前方（或后方）的几个点连续。下面上代码：
        此中 L_Border 和 R_Border 是存储一维边线的数组。

1. /**
2. * 函数功能：      寻找上转左拐点
3. * 特殊说明：      通过检测X值的跳变判定拐点的存在
4. *                 寻找方式为遍历一维数组边线的每个点
5. * 形  参：        uint8 Region_Start      起始行
6. *                 uint8 Region_End        截止行
7. *
8. * 示例：          Get_L_Up_Turn_Left_Point_2(2, 57);
9. * 返回值：        无
10. */
11. uint8 L_Up_Turn_Left_Point_2[2] = {0};
12. uint8 L_Up_Turn_Left_Point_Flag_2 = 0;
13. uint8 L_Up_Turn_Left_Point_Position_2 = 0;
14. void Get_L_Up_Turn_Left_Point_2(uint8 Region_Start, uint8 Region_End)
15. {
16.     uint8 i = 0;
17.     L_Up_Turn_Left_Point_2[0] = 0;
18.     L_Up_Turn_Left_Point_2[1] = 0;
19.     L_Up_Turn_Left_Point_Position_2 = 0;
20.     if(Region_Start <= Region_End)
21.     {
22.         for(i = Region_Start; i <= Region_End; i++)
23.         {
24.             if(L_Border[i] - L_Border[i - 1] >= 6)
25.             {
26.                 if(L_Border[i + 1] - L_Border[i - 2] >= 6)
27.                 {
28.                     if(L_Border[i - 1] == 2 && L_Border[i - 2] == 2 && L_Border[i] != 2)
29.                     {
30.                         L_Up_Turn_Left_Point_2[0] = L_Border[i];
31.                         L_Up_Turn_Left_Point_2[1] = i;
32.                         L_Up_Turn_Left_Point_Flag_2 = 1;
33.                         L_Up_Turn_Left_Point_Position_2 = i;
34.                     }
35.                 }
36.             }
37.         }
38.     }
39. }
42. /**
43. * 函数功能：      寻找右转上拐点
44. * 特殊说明：      通过检测X值的跳变判定拐点的存在
45. *                 寻找方式为遍历一维数组边线的每个点
46. * 形  参：        uint8 Region_Start      起始行
47. *                 uint8 Region_End        截止行
48. *
49. * 示例：          Get_L_Right_Turn_Up_Point_2(2, 57);
50. * 返回值：        无
51. */
52. uint8 L_Right_Turn_Up_Point_2[2] = {0};
53. uint8 L_Right_Turn_Up_Point_Flag_2 = 0;
54. uint8 L_Right_Turn_Up_Point_Position_2 = 0;
55. void Get_L_Right_Turn_Up_Point_2(uint8 Region_Start, uint8 Region_End)
56. {
57.     uint8 i = 0;
58.     L_Right_Turn_Up_Point_2[0] = 0;
59.     L_Right_Turn_Up_Point_2[1] = 0;
60.     L_Right_Turn_Up_Point_Position_2 = 0;
61.     if(Region_Start <= Region_End)
62.     {
63.         for(i = Region_Start; i <= Region_End; i++)
64.         {
65.             if(L_Border[i] - L_Border[i + 1] >= 6)
66.             {
67.                 if(L_Border[i - 1] - L_Border[i + 2] >= 6)
68.                 {
69.                     if(L_Border[i + 1] == 2 && L_Border[i + 2] == 2 && L_Border[i] != 2)
70.                     {
71.                         L_Right_Turn_Up_Point_2[0] = L_Border[i];
72.                         L_Right_Turn_Up_Point_2[1] = i;
73.                         L_Right_Turn_Up_Point_Flag_2 = 1;
74.                         L_Right_Turn_Up_Point_Position_2 = i;
75.                     }
76.                 }
77.             }
78.         }
79.     }
80. }
83. /**
84. * 函数功能：      寻找上转右拐点
85. * 特殊说明：      通过检测X值的跳变判定拐点的存在
86. *                 寻找方式为遍历一维数组边线的每个点
87. * 形  参：        uint8 Region_Start      起始行
88. *                 uint8 Region_End        截止行
89. *
90. * 示例：          Get_R_Up_Turn_Right_Point_2(2, 57);
91. * 返回值：        无
92. */
93. uint8 R_Up_Turn_Right_Point_2[2] = {0};
94. uint8 R_Up_Turn_Right_Point_Flag_2 = 0;
95. uint8 R_Up_Turn_Right_Point_Position_2 = 0;
96. void Get_R_Up_Turn_Right_Point_2(uint8 Region_Start, uint8 Region_End)
97. {
98.     uint8 i = 0;
99.     R_Up_Turn_Right_Point_2[0] = 0;
100.     R_Up_Turn_Right_Point_2[1] = 0;
101.     R_Up_Turn_Right_Point_Position_2 = 0;
102.     if(Region_Start <= Region_End)
103.     {
104.         for(i = Region_Start; i <= Region_End; i++)
105.         {
106.             if((int8)R_Border[i] - (int8)R_Border[i - 1] <= -6)
107.             {
108.                 if((int8)R_Border[i + 1] - (int8)R_Border[i - 2] <= -6)
109.                 {
110.                     if(R_Border[i - 1] == 77 && R_Border[i - 2] == 77 && R_Border[i] != 77)
111.                     {
112.                         R_Up_Turn_Right_Point_2[0] = R_Border[i];
113.                         R_Up_Turn_Right_Point_2[1] = i;
114.                         R_Up_Turn_Right_Point_Flag_2 = 1;
115.                         R_Up_Turn_Right_Point_Position_2  = i;
116.                     }
117.                 }
118.             }
119.         }
120.     }
121. }
124. /**
125. * 函数功能：      寻找左转上拐点
126. * 特殊说明：      通过检测X值的跳变判定拐点的存在
127. *                 寻找方式为遍历一维数组边线的每个点
128. * 形  参：        uint8 Region_Start      起始行
129. *                 uint8 Region_End        截止行
130. *
131. * 示例：          Get_R_Left_Turn_Up_Point_2(2, 57);
132. * 返回值：        无
133. */
134. uint8 R_Left_Turn_Up_Point_2[2] = {0};
135. uint8 R_Left_Turn_Up_Point_Flag_2 = 0;
136. uint8 R_Left_Turn_Up_Point_Position_2 = 0;
137. void Get_R_Left_Turn_Up_Point_2(uint8 Region_Start, uint8 Region_End)
138. {
139.     uint8 i = 0;
140.     if(Region_Start <= Region_End)
141.     {
142.         for(i = Region_Start; i <= Region_End; i++)
143.         {
144.             if((int8)R_Border[i] - (int8)R_Border[i + 1] <= -6)
145.             {
146.                 if((int8)R_Border[i - 1] - (int8)R_Border[i + 2] <= -6)
147.                 {
148.                     if(R_Border[i + 1] == 77 && R_Border[i + 2] == 77 && R_Border[i] != 77)
149.                     {
150.                         R_Left_Turn_Up_Point_2[0] = R_Border[i];
151.                         R_Left_Turn_Up_Point_2[1] = i;
152.                         R_Left_Turn_Up_Point_Flag_2 = 1;
153.                         R_Left_Turn_Up_Point_Position_2 = i;
154.                     }
155.                 }
156.             }
157.         }
158.     }
159. }

复制代码

3、组合判断拐点

        可先用生长方向找拐点，再用X坐标跳变强化判断拐点，代码如下：

1. //组合寻拐点
2. uint8 L_Up_Turn_Left_Point[2] = {0};
3. uint8 L_Up_Turn_Left_Point_Flag = 0;
4. void Get_L_Up_Turn_Left_Point(void)
5. {
6.     L_Up_Turn_Left_Point_Flag = 0;
7.     Get_L_Up_Turn_Left_Point_1(2);
8.     if(L_Up_Turn_Left_Point_Flag_1 == 1)
9.     {
10.         L_Up_Turn_Left_Point_Flag_1 = 0;
11.         Get_L_Up_Turn_Left_Point_2(L_Up_Turn_Left_Point_1[1] - 7, L_Up_Turn_Left_Point_1[1] + 7);
12.         {
13.             if(L_Up_Turn_Left_Point_Flag_2 == 1)
14.             {
15.                 L_Up_Turn_Left_Point_Flag_2 = 0;
16.                 L_Up_Turn_Left_Point_Flag = 1;
17.                 L_Up_Turn_Left_Point[0] = L_Up_Turn_Left_Point_1[0];
18.                 L_Up_Turn_Left_Point[1] = L_Up_Turn_Left_Point_1[1];
19.             }
20.         }
21.     }
22. }
25. uint8 L_Right_Turn_Up_Point[2] = {0};
26. uint8 L_Right_Turn_Up_Point_Flag = 0;
27. void Get_L_Right_Turn_Up_Point(void)
28. {
29.     L_Right_Turn_Up_Point_Flag = 0;
30.     Get_L_Right_Turn_Up_Point_1(2);
31.     if(L_Right_Turn_Up_Point_Flag_1 == 1)
32.     {
33.         L_Right_Turn_Up_Point_Flag_1 = 0;
34.         Get_L_Right_Turn_Up_Point_2(L_Right_Turn_Up_Point_1[1] - 7, L_Right_Turn_Up_Point_1[1] + 7);
35.         {
36.             if(L_Right_Turn_Up_Point_Flag_2 == 1)
37.             {
38.                 L_Right_Turn_Up_Point_Flag_2 = 0;
39.                 L_Right_Turn_Up_Point_Flag = 1;
40.                 L_Right_Turn_Up_Point[0] = L_Right_Turn_Up_Point_1[0];
41.                 L_Right_Turn_Up_Point[1] = L_Right_Turn_Up_Point_1[1];
42.             }
43.         }
44.     }
45. }
48. uint8 R_Up_Turn_Right_Point[2] = {0};
49. uint8 R_Up_Turn_Right_Point_Flag = 0;
50. void Get_R_Up_Turn_Right_Point(void)
51. {
52.     R_Up_Turn_Right_Point_Flag = 0;
53.     Get_R_Up_Turn_Right_Point_1(2);
54.     if(R_Up_Turn_Right_Point_Flag_1 == 1)
55.     {
56.         R_Up_Turn_Right_Point_Flag_1 = 0;
57.         Get_R_Up_Turn_Right_Point_2(R_Up_Turn_Right_Point_1[1] - 7, R_Up_Turn_Right_Point_1[1] + 7);
58.         {
59.             if(R_Up_Turn_Right_Point_Flag_2 == 1)
60.             {
61.                 R_Up_Turn_Right_Point_Flag_2 = 0;
62.                 R_Up_Turn_Right_Point_Flag = 1;
63.                 R_Up_Turn_Right_Point[0] = R_Up_Turn_Right_Point_1[0];
64.                 R_Up_Turn_Right_Point[1] = R_Up_Turn_Right_Point_1[1];
65.             }
66.         }
67.     }
68. }
71. uint8 R_Left_Turn_Up_Point[2] = {0};
72. uint8 R_Left_Turn_Up_Point_Flag = 0;
73. void Get_R_Left_Turn_Up_Point(void)
74. {
75.     R_Left_Turn_Up_Point_Flag = 0;
76.     Get_R_Left_Turn_Up_Point_1(2);
77.     if(R_Left_Turn_Up_Point_Flag_1 == 1)
78.     {
79.         R_Left_Turn_Up_Point_Flag_1 = 0;
80.         Get_R_Left_Turn_Up_Point_2(R_Left_Turn_Up_Point_1[1] - 7, R_Left_Turn_Up_Point_1[1] + 7);
81.         {
82.             if(R_Left_Turn_Up_Point_Flag_2 == 1)
83.             {
84.                 R_Left_Turn_Up_Point_Flag_2 = 0;
85.                 R_Left_Turn_Up_Point_Flag = 1;
86.                 R_Left_Turn_Up_Point[0] = R_Left_Turn_Up_Point_1[0];
87.                 R_Left_Turn_Up_Point[1] = R_Left_Turn_Up_Point_1[1];
88.             }
89.         }
90.     }
91. }

复制代码

4、逆透视求角度找拐点

        使用此方法需先求得逆透视变动数组，再使用逆透视求取角度。逆透视实现方法可以参考之前的文章：智能车摄像头开源—5 逆透视处置惩罚

1. //-------------------------------------------------------------------------------------------------------------------
2. //  @brief      逆透视知三点求形成的角度
3. //  @param      Ax，Ay      下边点
4. //  @param      Bx，By      要求角度的一点
5. //  @param      Cx，Cy      上边点
6. //  @return
7. //  @since      v1.0
8. //  Sample usage:
9. //  注：使用时传入原图像的三个点，不可传入逆透视图像的三个点
10. //-------------------------------------------------------------------------------------------------------------------
11. float Get_Turn_Point_Angle(uint8 Ax, uint8 Ay, uint8 Bx, uint8 By, uint8 Cx, uint8 Cy)
12. {
14.     float BA = 0.00;//向量BA的模
15.     float BC = 0.00;
16.     float SBA_BC = 0.00;//向量点乘的值
17.     float Angle = 0.00;
19.     uint8 AX = Back_Inverse_Matrix_Row[Ay * Image_X + Ax];
20.     uint8 AY = Back_Inverse_Matrix_Col[Ay * Image_X + Ax];
21.     uint8 BX = Back_Inverse_Matrix_Row[By * Image_X + Bx];
22.     uint8 BY = Back_Inverse_Matrix_Col[By * Image_X + Bx];
23.     uint8 CX = Back_Inverse_Matrix_Row[Cy * Image_X + Cx];
24.     uint8 CY = Back_Inverse_Matrix_Col[Cy * Image_X + Cx];
26.     BA = sqrt((float)((AX-BX)*(AX-BX)+(AY-BY)*(AY-BY)));
27.     BC = sqrt((float)((CX-BX)*(CX-BX)+(CY-BY)*(CY-BY)));
29.     SBA_BC = (float)((AX-BX)*(CX-BX)+(AY-BY)*(CY-BY));
31.     Angle =  acos(SBA_BC * 1.00/ (BA * BC));
33.     return Angle * 57.3f;
34. }
37. //逆透视后的图像拐点求角度，传入已经逆透视后的三个点
38. float I_Get_Turn_Point_Angle(uint8 Ax, uint8 Ay, uint8 Bx, uint8 By, uint8 Cx, uint8 Cy)
39. {
40.     float BA = 0.00;//向量BA的模
41.     float BC = 0.00;
42.     float SBA_BC = 0.00;//向量点乘的值
43.     float Angle = 0.00;
45.     BA = sqrt((float)((Ax-Bx)*(Ax-Bx)+(Ay-By)*(Ay-By)));
46.     BC = sqrt((float)((Cx-Bx)*(Cx-Bx)+(Cy-By)*(Cy-By)));
48.     SBA_BC = (float)((Ax-Bx)*(Cx-Bx)+(Ay-By)*(Cy-By));
50.     Angle =  acos(SBA_BC * 1.00/ (BA * BC));
52.     return Angle * 57.3f;
53. }

复制代码

        寻找时对所有的二维边线点举行遍历寻找。至于三个点的输入顺序，已经知道中间点，别的两个点可以实验两次即可确定。经本人实验，此方法并不稳定，以是后续未使用，便不过多赘述，如有兴趣可自行研究。

三、边界点求取

        由于使用爬线算法，提前在图像中补了黑框，以是在碰到十字、弯道等图像时，不可避免的会有部分边线落在黑框上，在此我称之为边界点，即位于左右两侧边界上的点。
        找出左右两侧边界上的点的个数，可用于判断弯道（一侧有边界点，另一侧没有边界点）、判断十字（左右两侧都有边界点），当然还有环岛等，要根据详细环境去使用，以下给出代码：

1. /**
2. * 函数功能：      寻找左侧边线的边界点个数
3. * 特殊说明：      无
4. * 形  参：        无
5. *
6. * 示例：          Get_L_Border_Point_Num();
7. * 返回值：        无
8. */
9. uint8 L_Border_Point_Num = 0;
10. uint8 L_UP_Border_Point_Num = 0;    //记录位于顶部黑框上的边界点个数
11. void Get_L_Border_Point_Num(void)
12. {
13.     uint8 i = 0;
14.     L_Border_Point_Num = 0;
15.     L_UP_Border_Point_Num = 0;
17.     for(i = Y_Meet; i < L_Line[0][1]; i++)
18.     {
19.         if(L_Border[i] == 2)    //左右两侧用一维边线即可，2是我的左侧边界
20.         {
21.             L_Border_Point_Num ++;
22.         }
23.     }
24.     for(i = 40; i < L_Statics - 2; i++)
25.     {
26.         if(L_Line[i][1] == 2)   //顶部的边界点使用二维边线
27.         {
28.             L_UP_Border_Point_Num ++;
29.         }
30.     }
31. }
33. /**
34. * 函数功能：      寻找右侧边线的边界点个数
35. * 特殊说明：      无
36. * 形  参：        无
37. *
38. * 示例：          Get_R_Border_Point_Num();
39. * 返回值：        无
40. */
41. uint8 R_Border_Point_Num = 0;
42. uint8 R_UP_Border_Point_Num = 0;
43. void Get_R_Border_Point_Num(void)
44. {
45.     uint8 i = 0;
46.     R_Border_Point_Num = 0;
47.     R_UP_Border_Point_Num = 0;
49.     for(i = Y_Meet; i < R_Line[0][1]; i++)
50.     {
51.         if(R_Border[i] == 77)    //77是我的右侧边界
52.         {
53.             R_Border_Point_Num ++;
54.         }
55.     }
56.     for(i = 40; i < R_Statics - 2; i++)
57.     {
58.         if(R_Line[i][1] == 2)
59.         {
60.             R_UP_Border_Point_Num ++;
61.         }
62.     }
63. }

复制代码

四、对位边界行

        当图像的某一行，它的左侧边线点位于左侧边界上的同时，右侧边线点也位于右侧边界上，那我称之为对位边界行，可以用来辅助判断十字路口，正常环境下只有十字路口才会出现数个对位边界行。

1. /**
2. * 函数功能：      寻找对位边界行的个数
3. * 特殊说明：      无
4. * 形  参：        无
5. *
6. * 示例：          Get_Relative_Border_Point_Num();
7. * 返回值：        无
8. */
9. uint8 Relative_Border_Point_Num = 0;        //存储对位边界行的个数
10. void Get_Relative_Border_Point_Num(void)
11. {
12.     uint8 i = 0;
13.     Relative_Border_Point_Num = 0;  //函数每张图像边线都调用一次，每调用一次都要清零
14.     for(i = Y_Meet; i < L_Start_Point[1]; i++)
15.     {
16.         if(L_Border[i] == 2 && R_Border[i] == 77)   //2为左侧边界的X坐标，77为右侧边界
17.         {
18.             Relative_Border_Point_Num ++;
19.         }
20.     }
21. }

复制代码

五、圆弧拐点求取

        圆弧拐点常见于小S弯道以及环岛，可以用于元素判断。判断直角拐点的时候轻易与圆弧拐点误判，但有差别的需求场景，详细本身真正纯熟使用后就可理解。
        圆弧拐点的判断原理为找一段连续点中最突出的那个点，比方左侧内凹的圆弧拐点，其前方的数个点X坐标一直在增大，其之后的数个点X坐标值一直在减小，同时使用生长方向举行辅助判断，生长方向使用的是本人八向迷宫算法中的方向，可查看之前文章去了解：
智能车摄像头开源—1.2核心算法：自适应八向迷宫（下）

1. /**
2. * 函数功能：      寻找左侧圆弧拐点
3. * 特殊说明：      无
4. * 形  参：        无
5. *
6. * 示例：          Get_L_Arc_Turn_Point();
7. * 返回值：        无
8. */
9. uint8 L_Arc_Turn_Point[3][2] = {{0}};   //最多找三个就足够使用
10. uint8 L_Arc_Turn_Point_Flag = 0;    //找到一个就挂出标志位
11. uint8 L_Arc_Turn_Point_Num = 0;     //记录找到的个数
12. void Get_L_Arc_Turn_Point(void)
13. {
14.     uint8 i = 0;
15.     L_Arc_Turn_Point_Flag = 0;
17.     for(i = 0; i < 3; i++)      //每次调用先清零
18.     {
19.         L_Arc_Turn_Point_Num = 0;
20.         L_Arc_Turn_Point[i][0] = 0;
21.         L_Arc_Turn_Point[i][1] = 0;
22.     }
24.     for(i =7; i < (L_Statics - 5); i++)
25.     {
26.         if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 7)))
27.         {
28.             if((L_Line[i][0] > L_Line[i - 7][0]) && (L_Line[i][0] > L_Line[i - 4][0]) && (L_Line[i][0] >= L_Line[i - 1][0]) &&
29.                (L_Line[i][0] >= L_Line[i + 1][0]) && (L_Line[i][0] > L_Line[i + 3][0]) && (L_Line[i][0] > L_Line[i + 5][0]) &&
30.                (L_Grow_Dir[i + 1] == -2 || L_Grow_Dir[i + 1] == 1) && (L_Grow_Dir[i + 3] == -2 || L_Grow_Dir[i + 3] == 1) &&
31.                (L_Grow_Dir[i - 1] == 1 || L_Grow_Dir[i + 1] == 4) && (L_Grow_Dir[i - 3] == 1 || L_Grow_Dir[i - 3] == 4))                    //左侧内凹
32.             {
33.                 L_Arc_Turn_Point[L_Arc_Turn_Point_Num][0] = L_Line[i][0];
34.                 L_Arc_Turn_Point[L_Arc_Turn_Point_Num][1] = L_Line[i][1];
35.                 L_Arc_Turn_Point_Flag = 1;
36.                 L_Arc_Turn_Point_Num ++;
37.                 i += 15;    //每个圆弧拐点前后几个点可能都满足圆弧拐点的条件，所以找到后加15个点
38.                 if(L_Arc_Turn_Point_Num == 3)
39.                 {
40.                     break;
41.                 }
42.             }
43.         }
44.     }
46.     for(i =7; i < (L_Statics - 5); i++)
47.     {
48.         if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 7)))
49.         {
51.             if((L_Line[i][0] < L_Line[i - 7][0]) && (L_Line[i][0] < L_Line[i - 4][0]) && (L_Line[i][0] <= L_Line[i - 1][0]) &&
52.                (L_Line[i][0] <= L_Line[i + 1][0]) && (L_Line[i][0] < L_Line[i + 3][0]) && (L_Line[i][0] < L_Line[i + 5][0]) &&
53.                (L_Grow_Dir[i + 1] == 4 || L_Grow_Dir[i + 1] == 1) && (L_Grow_Dir[i + 3] == 4 || L_Grow_Dir[i + 3] == 1) &&
54.                (L_Grow_Dir[i - 1] == 1 || L_Grow_Dir[i + 1] == -2) && (L_Grow_Dir[i - 3] == 1 || L_Grow_Dir[i - 3] == -2))                    //左侧外凸
55.             {
56.                 L_Arc_Turn_Point[L_Arc_Turn_Point_Num][0] = L_Line[i][0];
57.                 L_Arc_Turn_Point[L_Arc_Turn_Point_Num][1] = L_Line[i][1];
58.                 L_Arc_Turn_Point_Flag = 1;
59.                 L_Arc_Turn_Point_Num ++;
60.                 i += 15;
61.                 if(L_Arc_Turn_Point_Num == 3)
62.                 {
63.                     break;
64.                 }
65.             }
66.         }
67.     }
68. }
71. /**
72. * 函数功能：      寻找右侧圆弧拐点
73. * 特殊说明：      无
74. * 形  参：        无
75. *
76. * 示例：          Get_R_Arc_Turn_Point();
77. * 返回值：        无
78. */
79. uint8 R_Arc_Turn_Point[3][2] = {{0}};
80. uint8 R_Arc_Turn_Point_Flag = 0;
81. uint8 R_Arc_Turn_Point_Num = 0;
82. void Get_R_Arc_Turn_Point(void)
83. {
84.     uint8 i = 0;
85.     R_Arc_Turn_Point_Flag = 0;
86.     for(i = 0; i < 3; i++)
87.     {
88.         R_Arc_Turn_Point_Num = 0;
89.         R_Arc_Turn_Point[i][0] = 0;
90.         R_Arc_Turn_Point[i][1] = 0;
91.     }
93.     for(i = 7; i < (R_Statics - 5); i++)
94.     {
95.         if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
96.         {
97.             if((R_Line[i][0] < R_Line[i - 7][0]) && (R_Line[i][0] < R_Line[i - 4][0]) && (R_Line[i][0] <= R_Line[i - 1][0]) &&
98.                (R_Line[i][0] <= R_Line[i + 1][0]) && (R_Line[i][0] < R_Line[i + 3][0]) && (R_Line[i][0] < R_Line[i + 5][0]) &&
99.                (R_Grow_Dir[i + 1] == 4 || R_Grow_Dir[i + 1] == 1) && (R_Grow_Dir[i + 3] == 4 || R_Grow_Dir[i + 3] == 1) &&
100.                (R_Grow_Dir[i - 1] == 1 || R_Grow_Dir[i + 1] == -2) && (R_Grow_Dir[i - 3] == 1 || R_Grow_Dir[i - 3] == -2))                    //右侧内凹
101.             {
102.                 R_Arc_Turn_Point[R_Arc_Turn_Point_Num][0] = R_Line[i][0];
103.                 R_Arc_Turn_Point[R_Arc_Turn_Point_Num][1] = R_Line[i][1];
104.                 R_Arc_Turn_Point_Flag = 1;
105.                 R_Arc_Turn_Point_Num ++;
106.                 i += 15;
107.                 if(R_Arc_Turn_Point_Num == 3)
108.                 {
109.                     break;
110.                 }
111.             }
112.         }
113.     }
114.     for(i = 7; i < (R_Statics - 5); i++)
115.     {
116.         if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
117.         {
118.             if((R_Line[i][0] > R_Line[i - 7][0]) && (R_Line[i][0] > R_Line[i - 4][0]) && (R_Line[i][0] >= R_Line[i - 1][0]) &&
119.                (R_Line[i][0] >= R_Line[i + 1][0]) && (R_Line[i][0] > R_Line[i + 3][0]) && (R_Line[i][0] > R_Line[i + 5][0]) &&
120.                (R_Grow_Dir[i + 1] == -2 || R_Grow_Dir[i + 1] == 1) && (R_Grow_Dir[i + 3] == -2 || R_Grow_Dir[i + 3] == 1) &&
121.                (R_Grow_Dir[i - 1] == 1 || R_Grow_Dir[i + 1] == 4) && (R_Grow_Dir[i - 3] == 1 || R_Grow_Dir[i - 3] == 4))                    //右侧外凸
122.             {
123.                 R_Arc_Turn_Point[R_Arc_Turn_Point_Num][0] = R_Line[i][0];
124.                 R_Arc_Turn_Point[R_Arc_Turn_Point_Num][1] = R_Line[i][1];
125.                 R_Arc_Turn_Point_Flag = 1;
126.                 R_Arc_Turn_Point_Num ++;
127.                 i += 15;
128.                 if(R_Arc_Turn_Point_Num == 3)
129.                 {
130.                     break;
131.                 }
132.             }
133.         }
134.     }
135. }

复制代码

六、求每侧边线斜率及截距

        将每侧边线由中间点分割为两段（后称为一段和二段），分别计算一段、二段和整条边线的斜率以及截距（一维边线），然后将三者的斜率举行比对。当一段斜率近似等于二段斜率，一段斜率近似等于整段斜率，二段斜率近似等于整段斜率时，可判断为该侧边线为直线，用于直道的判断。

1. /**
2. * 函数功能：      最小二乘法计算斜率
3. * 特殊说明：      无
4. * 形  参：        uint8 begin                输入起点
5. *                 uint8 end                  输入终点
6. *                 uint8 *border              输入需要计算斜率的一维边线数组
7. * 示例：          Slope_Calculate(start, end, border);
8. * 返回值：        Result    计算出的斜率
9. */
10. float Slope_Calculate(uint8 begin, uint8 end, uint8 *border)    //注：begin 必须小于 end,一般 begin 位于图像上方， end 位于图像下方
11. {
12.     float X_Sum = 0, Y_Sum = 0, XY_Sum = 0, X2_Sum = 0;
13.     int16 i = 0;
14.     float Result = 0;
15.     static float Result_Last;
17.     for(i = begin; i < end ; i++)
18.     {
19.         X_Sum += (float)i;
20.         Y_Sum += (float)border[i];
21.         XY_Sum += (float)i * (border[i]);
22.         X2_Sum += (float)i * i;
23.     }
25.     if((end - begin) * X2_Sum - X_Sum * X_Sum)      //防止为0的情况出现
26.     {
27.         Result = ((float)(end - begin) * XY_Sum - X_Sum * Y_Sum) / ((float)(end - begin) * X2_Sum - X_Sum * X_Sum);
28.         Result_Last = Result;
29.     }
30.     else
31.     {
32.         Result = Result_Last;
33.     }
34.     return Result;
35. }
38. /**
39. * 函数功能：      计算斜率截距
40. * 特殊说明：      调用最小二乘法计算斜率
41. * 形  参：        uint8 start                输入起点
42. *                 uint8 end                  输入终点
43. *                 uint8 *border              输入需要计算斜率的一维边线数组
44. *                 float *slope_rate          存储斜率的变量地址
45. *                 float *intercept           存储截距的变量地址
46. * 示例：          Calculate_Slope_Intercept(start, end, L_Border, &L_Straightaway_Lope_Rate_C, &L_Intercept);
47. * 返回值：        无
48. */
49. void Calculate_Slope_Intercept(uint8 start, uint8 end, uint8 *border, float *slope_rate, float *intercept)
50. {
51.     uint16 i, Num = 0;
52.     uint16 X_Sum = 0, Y_Sum = 0;
53.     float Y_Average = 0, X_Average = 0;
55.     for(i = start; i < end; i++)
56.     {
57.         X_Sum += i;
58.         Y_Sum += border[i];
59.         Num ++;
60.     }
62.     if(Num)
63.     {
64.         X_Average = (float)(X_Sum / Num);
65.         Y_Average = (float)(Y_Sum / Num);
66.     }
68.     *slope_rate = Slope_Calculate(start, end, border);
69.     *intercept = (float)(Y_Average - (*slope_rate) * X_Average);
70. }
73. float Max_Slope_Dif_Thre = 0.6;     //实测得出，并未使用
74. float Min_Slope_Dif_Thre = 0.15;    //实测得出，当两段线段的斜率低于这个阈值时，可认为斜率相等
75. //斜率和截距
76. float R_Straightaway_Lope_Rate_A = 0;   //第一段的斜率
77. float R_Straightaway_Lope_Rate_B = 0;   //第二段的斜率
78. float R_Straightaway_Lope_Rate_C = 0;   //整段边线的斜率
79. float R_Intercept = 0;      //整段边线的截距
80. uint8 R_Straight_Flag = 0;      //右侧边线是否为直线标志位
82. /**
83. * 函数功能：      求右侧边线斜率截距，并判断是否为直线
84. * 特殊说明：      无
85. * 形  参：        uint8 start          //右侧边线起始点Y坐标
86. *                 uint8 end            //右侧边线终止点Y坐标
87. *
88. * 示例：          Get_R_Intercept_And_Slope(R_Line[R_Statics][1] + 5, R_Line[0][1]);
89. * 返回值：        无
90. */
91. void Get_R_Intercept_And_Slope(uint8 start, uint8 end)
92. {
93.     R_Straightaway_Lope_Rate_A = 0;
94.     R_Straightaway_Lope_Rate_B = 0;
95.     R_Straightaway_Lope_Rate_C = 0;
96.     R_Intercept = 0;
97.     R_Straight_Flag = 0;            //所有值先清零
99.     R_Straightaway_Lope_Rate_A = Slope_Calculate(start, ((end - start) / 2) + start, R_Border);
100.     R_Straightaway_Lope_Rate_B = Slope_Calculate(((end - start) / 2) + start, end, R_Border);       //计算两段斜率
101.     Calculate_Slope_Intercept(start, end, R_Border, &R_Straightaway_Lope_Rate_C, &R_Intercept);     //计算整段边线斜率和截距
103.     if(My_ABS_F(R_Straightaway_Lope_Rate_A - R_Straightaway_Lope_Rate_B) <= Min_Slope_Dif_Thre &&
104.        My_ABS_F(R_Straightaway_Lope_Rate_B - R_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre &&
105.        My_ABS_F(R_Straightaway_Lope_Rate_A - R_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre)      //判断是否为直线
106.     {
107.         R_Straight_Flag = 1;
108.     }
109. }
112. float L_Straightaway_Lope_Rate_A = 0;
113. float L_Straightaway_Lope_Rate_B = 0;
114. float L_Straightaway_Lope_Rate_C = 0;
115. float L_Intercept = 0;
116. uint8 L_Straight_Flag = 0;
118. /**
119. * 函数功能：      求左侧边线斜率截距，并判断是否为直线
120. * 特殊说明：      无
121. * 形  参：        uint8 start          //左侧边线起始点Y坐标
122. *                 uint8 end            //左侧边线终止点Y坐标
123. *
124. * 示例：          Get_L_Intercept_And_Slope(L_Line[L_Statics][1] + 5, L_Line[0][1]);
125. * 返回值：        无
126. */
127. void Get_L_Intercept_And_Slope(uint8 start, uint8 end)
128. {
129.     L_Straightaway_Lope_Rate_A = 0;
130.     L_Straightaway_Lope_Rate_B = 0;
131.     L_Straightaway_Lope_Rate_C = 0;
132.     L_Intercept = 0;
133.     L_Straight_Flag = 0;
135.     L_Straightaway_Lope_Rate_A = Slope_Calculate(start, ((end - start) / 2) + start, L_Border);
136.     L_Straightaway_Lope_Rate_B = Slope_Calculate(((end - start) / 2) + start, end, L_Border);
137.     Calculate_Slope_Intercept(start, end, L_Border, &L_Straightaway_Lope_Rate_C, &L_Intercept);
139.     if(My_ABS_F(L_Straightaway_Lope_Rate_A - L_Straightaway_Lope_Rate_B) <= Min_Slope_Dif_Thre &&
140.        My_ABS_F(L_Straightaway_Lope_Rate_B - L_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre &&
141.        My_ABS_F(L_Straightaway_Lope_Rate_A - L_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre)
142.     {
143.         L_Straight_Flag = 1;
144.     }
145. }

复制代码

七、拟合曲线计算方差

        当我们求出左右两侧一维边线的斜率和截距以后，可以用斜率和截距在图像中画出一条直线，然后将实际的赛道边线的每个点（一维边线）与拟合直线的对应行的点计算方差，整条边线的方差的大小反应了赛道边线的弯曲程度。
        但实际验证此方法时，效果不甚理想，会碰到各种特别环境，同时以下代码存在些许bug，作者到最终都没修复，以是更多是在此处给出一个思路，想用的话可以实验修修下列代码的bug：

1. //拟合曲线
2. int16 L_Fitting_Line[] = {0};
3. int16 R_Fitting_Line[] = {0};
4. //方差
5. float L_Variance = 0;
6. float R_Variance = 0;
8. /**
9. * 函数功能：      计算左右两侧边线的方差
10. * 特殊说明：      每三行计算一次，减少计算量，但获取的信息不那么精确，可以选择计算每一行
11. * 形  参：        无
12. *
13. * 示例：          Get_Fitting_Line_And_Variance();
14. * 返回值：        无
15. */
16. void Get_Fitting_Line_And_Variance(void)
17. {
18.     uint8 i = 0;
20.     for(i = 1; i <= 19; i ++)
21.     {
22.         L_Fitting_Line[i] = (int16)(L_Straightaway_Lope_Rate_C * (float)(i * 3) + L_Intercept); //y = kx+b(把XY轴交换下就能想通了)
23.         L_Fitting_Line[i] = Limit_16(L_Fitting_Line[i], X_Border_Min + 2, X_Border_Max - 2);
25.         R_Fitting_Line[i] = (int16)(R_Straightaway_Lope_Rate_C * (float)(i * 3) + R_Intercept);
26.         R_Fitting_Line[i] = Limit_16(R_Fitting_Line[i], X_Border_Min + 2, X_Border_Max - 2);
27.     }
29.     Get_Variance(L_Line[L_Statics][1] + 1, L_Line[0][1], L_Fitting_Line, L_Border, &L_Variance, 3);
30.     Get_Variance(R_Line[R_Statics][1] + 1, R_Line[0][1], R_Fitting_Line, R_Border, &R_Variance, 3);
31. }

复制代码

八、求取左右侧边线差值

        用每行右侧边线点的X坐标值减去对应行左侧边线点的X坐标值（一维边线），可以得到每行的差值（大概说每行赛道的宽度），由此可以用来判断十字等元素。比方十字：图像从下往上的边线差值先变小，后突然变大，后又变小，呈现出十字的趋势。
        以下代码每隔两行计算一行，并没有计算全部行的差值，但是计算每一行完全可行。

1. int8 Row_Difference[20] = {0};
2. uint8 Max_Row_Dif_Line_Num = 0;     //当某一行左侧边线点位于左侧边界上，右侧边线点位于右侧边线上，与对位边界行同理，可以记录下来
3. /**
4. * 函数功能：      寻找对位边界行的个数
5. * 特殊说明：      无
6. * 形  参：        无
7. *
8. * 示例：          Get_Relative_Border_Point_Num();
9. * 返回值：        无
10. */
11. void Get_Row_Difference(uint8 *l_border, uint8 *r_border)
12. {
13.     uint8 i = 0;
14.     Max_Row_Dif_Line_Num = 0;
15.     for(i = 0; i < 20; i ++)
16.     {
17.         Row_Difference[i] = (int8)(r_border[i * 3] - l_border[i * 3]);
18.         if(Row_Difference[i] == 75)
19.         {
20.             Max_Row_Dif_Line_Num ++;
21.         }
22.     }
23. }

复制代码

九、TOF测距

        部分组别使用TOF测距模块检测停滞、坡道等元素，那么就必要调用对应函数获取距离。逐飞官方手册中说明：测量间隔不可低于10ms。因此我们直接使用图像计数（收罗十张图像间隔绝对大于10ms，而且时间相对固定，可根据本身帧率恰当调整）

1.     if(Image_Num % 10 == 0)   //其中Image_Num 需在复制图像时加一，即没采集一张图像加一一次  
2.     {
3.         Barrier_Distance = TOF_Get_Distance_mm();
4.     }

复制代码

        
十、总函数调用

        将上述所有处置惩罚举行整合，放至一个函数内，每张图像调用一次函数便可获取所有信息。对于部分元素内部环境（比方环岛、十字等特别元素会被对应的循环接管），只需调用对应的函数获取所需的信息即可。顺序可随意

1. /**
2. * 函数功能：      获取所有信息
3. * 特殊说明：      无
4. * 形  参：        无
5. *
6. * 示例：          Get_Information();
7. * 返回值：        无
8. */
9. void Get_Information(void)
10. {
11.     Get_L_Border_Point_Num();
12.     Get_R_Border_Point_Num();
13.     Get_Relative_Border_Point_Num();
14.     Get_L_Up_Turn_Left_Point_1(2);
15.     Get_L_Right_Turn_Up_Point_1(2);
16.     Get_R_Up_Turn_Right_Point_1(2);
17.     Get_R_Left_Turn_Up_Point_1(2);
18.     Get_L_Arc_Turn_Point();
19.     Get_R_Arc_Turn_Point();
20.     Get_L_Intercept_And_Slope(L_Line[L_Statics][1] + 5, L_Line[0][1]);
21.     Get_R_Intercept_And_Slope(R_Line[R_Statics][1] + 5, R_Line[0][1]);
22.     Get_Fitting_Line_And_Variance();
23.     Get_Row_Difference(L_Border, R_Border);
24. }

复制代码

十一、总结

        以上处置惩罚后就完成了对于边线信息的提取，加上之前的处置惩罚，我们可以获取一张图像赛道的以下信息：
1、左右两侧边线的二维数组

• 每个点的X，Y坐标；
  
• 每个点的生长方向；
  
• 每个点的阈值；
  

2、每侧二维边线点的个数
3、左右两侧边线的一维数组

• 每个点的X，Y（数组下标）坐标；
  

4、每侧一维边线点的个数
5、所有二维边线点的均值阈值
6、左右两侧爬线相遇点的X，Y坐标
7、左右两侧边界点的个数
8、顶部边界点的个数
9、对位边界行的个数
10、上转左、上转右、右转上、左转上四个拐点坐标及标记位
11、每侧最多三个圆弧拐点及标记位
12、每侧边线的斜率、截距以及是否为直线标记位
13、每侧边线的方差（弯曲程度）
14、左右两侧边线的差值
15、停滞物（坡道）距离

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。