目录
一、前言
本文主要讲解二维数组边线的信息提取，其中主要包括：直角拐点、圆弧拐点、边界点个数等。
二、直角拐点求取
1、根据生长方向找拐点
不论是常见的八邻域求边线，还是本人优化过的八向迷宫求取边线，都可以获得每个边线点的生长方向，由此我们可以判断直角拐点。
因摄像头为斜视，因此获取图像中的直角拐点并非严格的直角拐点，而是锐角，由此我们根据生长方向判断拐点时，
此处基于本人的八向迷宫讲解拐点求取。八向迷宫的生长方向不同于八邻域的0~7，而是特殊的数字用来代表（可以参考之前的文章：智能车摄像头开源—1.2 核心算法：自适应八向迷宫（下））：
-2, 1, 4
-3, 3
-4,-1, 2直角拐点主要需求四类：上转左，上转右，左转上，右转上。这里的上、下、左、右为生长的大致方向，例如上转左拐点，即边线大体上向上生长，经过上转左拐点后，转为大体上向左生长。其中，上转左和右转上只会在左侧边线出现，而上转右，左转上只会出现在右侧边线（二维边线）。当遇到十字路口时（正入十字，斜入另说），会遇到上述四个拐点，循环找四个拐点给标志位，即可初步判出十字元素。不可太过绝对。但同时不可太过简单，否则导致误判，在一定程度上，误判比漏判后果更为严重。
读者可求取一张十字的图像，因摄像头斜视原因，会发现向上生长的边线为斜线，向左或向右生长的边线较趋近于平滑的直线
下基于八向迷宫的八个方向分别讲解四个拐点，每个拐点有三个判定严格程度，每个拐点需根据前后几个点的生长方向加以判断。
1. 左侧上转左拐点
宽松判断：前方几个点的生长方向为 -2 或 1 或 4 ，后方几个点生长方向为 -2 或 -3 或 -4；
中等判断：前方几个点的生长方向为 1 或 4 ，后方几个点生长方向为 -3 或 -4；
严格判断：前方几个点的生长方向为 1 或 4 ，后方几个点生长方向为 -3 ；
其中宽松判断会和圆弧拐点误判，中等判断常用，严格判断要求过高，根据自身需求调用。
/**
* 函数功能： 寻找上转左拐点
* 特殊说明： 分为三个严格等级，根据每个点的生长方向判断
* 寻找方式为遍历二维数组边线的每个点
* 形 参： uint8 Grade 选择判定严格等级，常用2，即中等等级
*
* 示例： Get_L_Up_Turn_Left_Point_1(2);
* 返回值： 无
*/
uint8 L_Up_Turn_Left_Point_1[2] = {0}; //存储上转左拐点的坐标
uint8 L_Up_Turn_Left_Point_Flag_1 = 0; //上转左拐点存在标志位，找到时置1
uint8 L_Up_Turn_Left_Point_Position_1 = 0; //记录位置，即是二维数组中的第几个点
float L_Up_Turn_Left_Point_Angle_1 = 0; //记录找到的拐点的角度（逆透视求取）
void Get_L_Up_Turn_Left_Point_1(uint8 Grade)
{
uint8 i = 0;
L_Up_Turn_Left_Point_Flag_1 = 0;
L_Up_Turn_Left_Point_1[0] = 0;
L_Up_Turn_Left_Point_1[1] = 0;
L_Up_Turn_Left_Point_Position_1 = 0;
L_Up_Turn_Left_Point_Angle_1 = 0; //将各个参数清零
switch(Grade)
{
case 1: //严格判断
{
for (i = 4; i < (L_Statics - 4); i++) //L_Statics 为左侧边线点个数
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2))) //L_Line为存储左侧边线的二维数组，拐点必须满足不在于左右两侧边界上
{
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) &&
(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存储每个边线点的生长方向
{
L_Up_Turn_Left_Point_1[0] = L_Line[i][0];
L_Up_Turn_Left_Point_1[1] = L_Line[i][1];
L_Up_Turn_Left_Point_Flag_1 = 1;
break;
}
}
}
break;
}
case 2: //中等判断
{
for (i = 4; i < (L_Statics - 4); i++)
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(L_Grow_Dir[i + 2] == -3 || L_Grow_Dir[i + 2] == -4) && (L_Grow_Dir[i + 4] == -3 || L_Grow_Dir[i + 4] == -4) &&
(L_Grow_Dir[i] == -2 || L_Grow_Dir[i] == -3 || L_Grow_Dir[i] == -4))
{
L_Up_Turn_Left_Point_1[0] = L_Line[i][0];
L_Up_Turn_Left_Point_1[1] = L_Line[i][1];
L_Up_Turn_Left_Point_Flag_1 = 1;
break;
}
}
}
break;
}
case 3: //宽松判断
{
for (i = 4; i < (L_Statics - 4); i++)
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(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) &&
(L_Grow_Dir[i] == -2 || L_Grow_Dir[i] == -3 || L_Grow_Dir[i] == -4))
{
L_Up_Turn_Left_Point_1[0] = L_Line[i][0];
L_Up_Turn_Left_Point_1[1] = L_Line[i][1];
L_Up_Turn_Left_Point_Flag_1 = 1;
break;
}
}
}
break;
}
}
if(L_Up_Turn_Left_Point_Flag_1 == 1)
{
L_Up_Turn_Left_Point_Position_1 = i; //记录拐点的位置
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]); //逆透视求取拐点角度，后续补充
}
}2. 左侧右转上拐点
宽松判断：前方几个点的生长方向为 2 或 3 或 4 ，后方几个点生长方向为 4 或 1 或 -2；
中等判断：前方几个点的生长方向为 2或 3 ，后方几个点生长方向为 4 或 1；
严格判断：前方几个点的生长方向为 3 ，后方几个点生长方向为 4 或 1 ；
与左侧上转左拐点同理，下面几个都不做过多讲述。
/**
* 函数功能： 寻找右转上拐点
* 特殊说明： 分为三个严格等级，根据每个点的生长方向判断
* 寻找方式为遍历二维数组边线的每个点
* 形 参： uint8 Grade 选择判定严格等级，常用2，即中等等级
*
* 示例： Get_L_Right_Turn_Up_Point_1(2);
* 返回值： 无
*/
uint8 L_Right_Turn_Up_Point_1[2] = {0};
uint8 L_Right_Turn_Up_Point_Flag_1 = 0;
uint8 L_Right_Turn_Up_Point_Position_1 = 0;
float L_Right_Turn_Up_Point_Angle_1 = 0;
void Get_L_Right_Turn_Up_Point_1(uint8 Grade)
{
uint8 i = 0;
L_Right_Turn_Up_Point_Flag_1 = 0;
L_Right_Turn_Up_Point_1[0] = 0;
L_Right_Turn_Up_Point_1[1] = 0;
L_Right_Turn_Up_Point_Position_1 = 0;
L_Right_Turn_Up_Point_Angle_1 = 0;
switch(Grade)
{
case 1:
{
for (i = 5; i < (L_Statics - 4); i++)
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(L_Grow_Dir[i - 2] == 3) && (L_Grow_Dir[i - 5] == 3) && (L_Grow_Dir[i] == 4 || L_Grow_Dir[i] == 1))
{
L_Right_Turn_Up_Point_1[0] = L_Line[i][0];
L_Right_Turn_Up_Point_1[1] = L_Line[i][1];
L_Right_Turn_Up_Point_Flag_1 = 1;
break;
}
}
}
break;
}
case 2:
{
for (i = 5; i < (L_Statics - 4); i++)
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(L_Grow_Dir[i - 2] == 2 || L_Grow_Dir[i - 2] == 3) && (L_Grow_Dir[i - 5] == 2 || L_Grow_Dir[i - 5] == 3) &&
(L_Grow_Dir[i] == 4 || L_Grow_Dir[i] == 1))
{
L_Right_Turn_Up_Point_1[0] = L_Line[i][0];
L_Right_Turn_Up_Point_1[1] = L_Line[i][1];
L_Right_Turn_Up_Point_Flag_1 = 1;
break;
}
}
}
break;
}
case 3:
{
for (i = 5; i < (L_Statics - 4); i++)
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(L_Grow_Dir[i + 7] == 4 || L_Grow_Dir[i + 7] == 1 || L_Grow_Dir[i + 7] == -2) &&
(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) &&
(L_Grow_Dir[i] == 4 || L_Grow_Dir[i] == 1))
{
L_Right_Turn_Up_Point_1[0] = L_Line[i][0];
L_Right_Turn_Up_Point_1[1] = L_Line[i][1];
L_Right_Turn_Up_Point_Flag_1 = 1;
break;
}
}
}
break;
}
}
if(L_Right_Turn_Up_Point_Flag_1 == 1)
{
L_Right_Turn_Up_Point_Position_1 = i;
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]);
}
}3. 右侧上转右拐点
宽松判断：前方几个点的生长方向为 -2 或 1或 4 ，后方几个点生长方向为 2或 3或 4；
中等判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 2或 3；
严格判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 3 ；
/**
* 函数功能： 寻找上转右拐点
* 特殊说明： 分为三个严格等级，根据每个点的生长方向判断
* 寻找方式为遍历二维数组边线的每个点
* 形 参： uint8 Grade 选择判定严格等级，常用2，即中等等级
*
* 示例： Get_R_Up_Turn_Right_Point_1(2);
* 返回值： 无
*/
uint8 R_Up_Turn_Right_Point_1[2] = {0};
uint8 R_Up_Turn_Right_Point_Flag_1 = 0;
uint8 R_Up_Turn_Right_Point_Position_1 = 0;
float R_Up_Turn_Right_Point_Angle_1 = 0;
void Get_R_Up_Turn_Right_Point_1(uint8 Grade)
{
uint8 i = 0;
R_Up_Turn_Right_Point_Flag_1 = 0;
R_Up_Turn_Right_Point_1[0] = 0;
R_Up_Turn_Right_Point_1[1] = 0;
R_Up_Turn_Right_Point_Position_1 = 0;
R_Up_Turn_Right_Point_Angle_1 = 0;
switch(Grade)
{
case 1:
{
for (i = 4; i < (R_Statics - 4); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(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))
{
R_Up_Turn_Right_Point_1[0] = R_Line[i][0];
R_Up_Turn_Right_Point_1[1] = R_Line[i][1];
R_Up_Turn_Right_Point_Flag_1 = 1;
break;
}
}
}
break;
}
case 2:
{
for (i = 4; i < (R_Statics - 4); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(R_Grow_Dir[i + 2] == 2 || R_Grow_Dir[i + 2] == 3) && (R_Grow_Dir[i + 4] == 2 || R_Grow_Dir[i + 4] == 3) &&
(R_Grow_Dir[i] == 2 || R_Grow_Dir[i] == 3 || R_Grow_Dir[i] == 4))
{
R_Up_Turn_Right_Point_1[0] = R_Line[i][0];
R_Up_Turn_Right_Point_1[1] = R_Line[i][1];
R_Up_Turn_Right_Point_Flag_1 = 1;
break;
}
}
}
break;
}
case 3:
{
for (i = 4; i < (R_Statics - 4); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(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) &&
(R_Grow_Dir[i] == 2 || R_Grow_Dir[i] == 3 || R_Grow_Dir[i] == 4))
{
R_Up_Turn_Right_Point_1[0] = R_Line[i][0];
R_Up_Turn_Right_Point_1[1] = R_Line[i][1];
R_Up_Turn_Right_Point_Flag_1 = 1;
break;
}
}
}
break;
}
}
if(R_Up_Turn_Right_Point_Flag_1 == 1)
{
R_Up_Turn_Right_Point_Position_1 = i;
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]);
}
}4. 右侧左转上拐点
宽松判断：前方几个点的生长方向为 -2 或 1或 4 ，后方几个点生长方向为 2或 3或 4；
中等判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 2或 3；
严格判断：前方几个点的生长方向为 -2或 1，后方几个点生长方向为 3 ；
/**
* 函数功能： 寻找左转上拐点
* 特殊说明： 分为三个严格等级，根据每个点的生长方向判断
* 寻找方式为遍历二维数组边线的每个点
* 形 参： uint8 Grade 选择判定严格等级，常用2，即中等等级
*
* 示例： Get_R_Left_Turn_Up_Point_1(2);
* 返回值： 无
*/
uint8 R_Left_Turn_Up_Point_1[2] = {0};
uint8 R_Left_Turn_Up_Point_Flag_1 = 0;
uint8 R_Left_Turn_Up_Point_Position_1 = 0;
float R_Left_Turn_Up_Point_Angle_1 = 0;
void Get_R_Left_Turn_Up_Point_1(uint8 Grade)
{
uint8 i =0;
R_Left_Turn_Up_Point_Flag_1 = 0;
R_Left_Turn_Up_Point_1[0] = 0;
R_Left_Turn_Up_Point_1[1] = 0;
R_Left_Turn_Up_Point_Position_1 = 0;
R_Left_Turn_Up_Point_Angle_1 = 0;
switch(Grade)
{
case 1:
{
for (i = 5; i < (R_Statics - 4); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(R_Grow_Dir[i - 2] == -3) && (R_Grow_Dir[i - 5] == -3) && (R_Grow_Dir[i] == -2 || R_Grow_Dir[i] == 1))
{
R_Left_Turn_Up_Point_1[0] = R_Line[i][0];
R_Left_Turn_Up_Point_1[1] = R_Line[i][1];
R_Left_Turn_Up_Point_Flag_1 = 1;
break;
}
}
}
break;
}
case 2:
{
for (i = 5; i < (R_Statics - 4); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(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))
{
R_Left_Turn_Up_Point_1[0] = R_Line[i][0];
R_Left_Turn_Up_Point_1[1] = R_Line[i][1];
R_Left_Turn_Up_Point_Flag_1 = 1;;
break;
}
}
}
break;
}
case 3:
{
for (i = 5; i < (R_Statics - 4); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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) &&
(R_Grow_Dir[i + 7] == -2 || R_Grow_Dir[i + 7] == 1 || R_Grow_Dir[i + 7] == 4) &&
(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))
{
R_Left_Turn_Up_Point_1[0] = R_Line[i][0];
R_Left_Turn_Up_Point_1[1] = R_Line[i][1];
R_Left_Turn_Up_Point_Flag_1 = 1;;
break;
}
}
}
break;
}
}
if(R_Left_Turn_Up_Point_Flag_1 == 1)
{
R_Left_Turn_Up_Point_Position_1 = i;
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]);
}
}2、根据X坐标跳变寻找拐点
X坐标跳变寻拐点，使用的是一维边线数组。对于一维边线数组来说，每行左右边线只会存储一个点，即每行只会有两个边线点，不同于二维数组每行图像可以记录多个点。当遇到十字路口时对比尤为明显。
例如在十字路口时，正常的二维数组应是如下情况：
但一维数组存储的边线，应是如下情况：（画成蓝线更明显）
可以对比上下两张图像，一维边线失去了横向的多个点，即每行每个边线只存储一个点，由此在四个拐点处，会出现当前拐点的X值与下一个点的（或前一个点的）X值出现较大变化，通过遍历每侧的一维边线每个点，可以找到四个拐点。同时需判定其与其前方（或后方）的几个点连续。下面上代码：
其中 L_Border 和 R_Border 是存储一维边线的数组。
/**
* 函数功能： 寻找上转左拐点
* 特殊说明： 通过检测X值的跳变判定拐点的存在
* 寻找方式为遍历一维数组边线的每个点
* 形 参： uint8 Region_Start 起始行
* uint8 Region_End 截止行
*
* 示例： Get_L_Up_Turn_Left_Point_2(2, 57);
* 返回值： 无
*/
uint8 L_Up_Turn_Left_Point_2[2] = {0};
uint8 L_Up_Turn_Left_Point_Flag_2 = 0;
uint8 L_Up_Turn_Left_Point_Position_2 = 0;
void Get_L_Up_Turn_Left_Point_2(uint8 Region_Start, uint8 Region_End)
{
uint8 i = 0;
L_Up_Turn_Left_Point_2[0] = 0;
L_Up_Turn_Left_Point_2[1] = 0;
L_Up_Turn_Left_Point_Position_2 = 0;
if(Region_Start <= Region_End)
{
for(i = Region_Start; i <= Region_End; i++)
{
if(L_Border[i] - L_Border[i - 1] >= 6)
{
if(L_Border[i + 1] - L_Border[i - 2] >= 6)
{
if(L_Border[i - 1] == 2 && L_Border[i - 2] == 2 && L_Border[i] != 2)
{
L_Up_Turn_Left_Point_2[0] = L_Border[i];
L_Up_Turn_Left_Point_2[1] = i;
L_Up_Turn_Left_Point_Flag_2 = 1;
L_Up_Turn_Left_Point_Position_2 = i;
}
}
}
}
}
}
/**
* 函数功能： 寻找右转上拐点
* 特殊说明： 通过检测X值的跳变判定拐点的存在
* 寻找方式为遍历一维数组边线的每个点
* 形 参： uint8 Region_Start 起始行
* uint8 Region_End 截止行
*
* 示例： Get_L_Right_Turn_Up_Point_2(2, 57);
* 返回值： 无
*/
uint8 L_Right_Turn_Up_Point_2[2] = {0};
uint8 L_Right_Turn_Up_Point_Flag_2 = 0;
uint8 L_Right_Turn_Up_Point_Position_2 = 0;
void Get_L_Right_Turn_Up_Point_2(uint8 Region_Start, uint8 Region_End)
{
uint8 i = 0;
L_Right_Turn_Up_Point_2[0] = 0;
L_Right_Turn_Up_Point_2[1] = 0;
L_Right_Turn_Up_Point_Position_2 = 0;
if(Region_Start <= Region_End)
{
for(i = Region_Start; i <= Region_End; i++)
{
if(L_Border[i] - L_Border[i + 1] >= 6)
{
if(L_Border[i - 1] - L_Border[i + 2] >= 6)
{
if(L_Border[i + 1] == 2 && L_Border[i + 2] == 2 && L_Border[i] != 2)
{
L_Right_Turn_Up_Point_2[0] = L_Border[i];
L_Right_Turn_Up_Point_2[1] = i;
L_Right_Turn_Up_Point_Flag_2 = 1;
L_Right_Turn_Up_Point_Position_2 = i;
}
}
}
}
}
}
/**
* 函数功能： 寻找上转右拐点
* 特殊说明： 通过检测X值的跳变判定拐点的存在
* 寻找方式为遍历一维数组边线的每个点
* 形 参： uint8 Region_Start 起始行
* uint8 Region_End 截止行
*
* 示例： Get_R_Up_Turn_Right_Point_2(2, 57);
* 返回值： 无
*/
uint8 R_Up_Turn_Right_Point_2[2] = {0};
uint8 R_Up_Turn_Right_Point_Flag_2 = 0;
uint8 R_Up_Turn_Right_Point_Position_2 = 0;
void Get_R_Up_Turn_Right_Point_2(uint8 Region_Start, uint8 Region_End)
{
uint8 i = 0;
R_Up_Turn_Right_Point_2[0] = 0;
R_Up_Turn_Right_Point_2[1] = 0;
R_Up_Turn_Right_Point_Position_2 = 0;
if(Region_Start <= Region_End)
{
for(i = Region_Start; i <= Region_End; i++)
{
if((int8)R_Border[i] - (int8)R_Border[i - 1] <= -6)
{
if((int8)R_Border[i + 1] - (int8)R_Border[i - 2] <= -6)
{
if(R_Border[i - 1] == 77 && R_Border[i - 2] == 77 && R_Border[i] != 77)
{
R_Up_Turn_Right_Point_2[0] = R_Border[i];
R_Up_Turn_Right_Point_2[1] = i;
R_Up_Turn_Right_Point_Flag_2 = 1;
R_Up_Turn_Right_Point_Position_2 = i;
}
}
}
}
}
}
/**
* 函数功能： 寻找左转上拐点
* 特殊说明： 通过检测X值的跳变判定拐点的存在
* 寻找方式为遍历一维数组边线的每个点
* 形 参： uint8 Region_Start 起始行
* uint8 Region_End 截止行
*
* 示例： Get_R_Left_Turn_Up_Point_2(2, 57);
* 返回值： 无
*/
uint8 R_Left_Turn_Up_Point_2[2] = {0};
uint8 R_Left_Turn_Up_Point_Flag_2 = 0;
uint8 R_Left_Turn_Up_Point_Position_2 = 0;
void Get_R_Left_Turn_Up_Point_2(uint8 Region_Start, uint8 Region_End)
{
uint8 i = 0;
if(Region_Start <= Region_End)
{
for(i = Region_Start; i <= Region_End; i++)
{
if((int8)R_Border[i] - (int8)R_Border[i + 1] <= -6)
{
if((int8)R_Border[i - 1] - (int8)R_Border[i + 2] <= -6)
{
if(R_Border[i + 1] == 77 && R_Border[i + 2] == 77 && R_Border[i] != 77)
{
R_Left_Turn_Up_Point_2[0] = R_Border[i];
R_Left_Turn_Up_Point_2[1] = i;
R_Left_Turn_Up_Point_Flag_2 = 1;
R_Left_Turn_Up_Point_Position_2 = i;
}
}
}
}
}
}3、组合判定拐点
可先用生长方向找拐点，再用X坐标跳变强化判定拐点，代码如下：
//组合寻拐点
uint8 L_Up_Turn_Left_Point[2] = {0};
uint8 L_Up_Turn_Left_Point_Flag = 0;
void Get_L_Up_Turn_Left_Point(void)
{
L_Up_Turn_Left_Point_Flag = 0;
Get_L_Up_Turn_Left_Point_1(2);
if(L_Up_Turn_Left_Point_Flag_1 == 1)
{
L_Up_Turn_Left_Point_Flag_1 = 0;
Get_L_Up_Turn_Left_Point_2(L_Up_Turn_Left_Point_1[1] - 7, L_Up_Turn_Left_Point_1[1] + 7);
{
if(L_Up_Turn_Left_Point_Flag_2 == 1)
{
L_Up_Turn_Left_Point_Flag_2 = 0;
L_Up_Turn_Left_Point_Flag = 1;
L_Up_Turn_Left_Point[0] = L_Up_Turn_Left_Point_1[0];
L_Up_Turn_Left_Point[1] = L_Up_Turn_Left_Point_1[1];
}
}
}
}
uint8 L_Right_Turn_Up_Point[2] = {0};
uint8 L_Right_Turn_Up_Point_Flag = 0;
void Get_L_Right_Turn_Up_Point(void)
{
L_Right_Turn_Up_Point_Flag = 0;
Get_L_Right_Turn_Up_Point_1(2);
if(L_Right_Turn_Up_Point_Flag_1 == 1)
{
L_Right_Turn_Up_Point_Flag_1 = 0;
Get_L_Right_Turn_Up_Point_2(L_Right_Turn_Up_Point_1[1] - 7, L_Right_Turn_Up_Point_1[1] + 7);
{
if(L_Right_Turn_Up_Point_Flag_2 == 1)
{
L_Right_Turn_Up_Point_Flag_2 = 0;
L_Right_Turn_Up_Point_Flag = 1;
L_Right_Turn_Up_Point[0] = L_Right_Turn_Up_Point_1[0];
L_Right_Turn_Up_Point[1] = L_Right_Turn_Up_Point_1[1];
}
}
}
}
uint8 R_Up_Turn_Right_Point[2] = {0};
uint8 R_Up_Turn_Right_Point_Flag = 0;
void Get_R_Up_Turn_Right_Point(void)
{
R_Up_Turn_Right_Point_Flag = 0;
Get_R_Up_Turn_Right_Point_1(2);
if(R_Up_Turn_Right_Point_Flag_1 == 1)
{
R_Up_Turn_Right_Point_Flag_1 = 0;
Get_R_Up_Turn_Right_Point_2(R_Up_Turn_Right_Point_1[1] - 7, R_Up_Turn_Right_Point_1[1] + 7);
{
if(R_Up_Turn_Right_Point_Flag_2 == 1)
{
R_Up_Turn_Right_Point_Flag_2 = 0;
R_Up_Turn_Right_Point_Flag = 1;
R_Up_Turn_Right_Point[0] = R_Up_Turn_Right_Point_1[0];
R_Up_Turn_Right_Point[1] = R_Up_Turn_Right_Point_1[1];
}
}
}
}
uint8 R_Left_Turn_Up_Point[2] = {0};
uint8 R_Left_Turn_Up_Point_Flag = 0;
void Get_R_Left_Turn_Up_Point(void)
{
R_Left_Turn_Up_Point_Flag = 0;
Get_R_Left_Turn_Up_Point_1(2);
if(R_Left_Turn_Up_Point_Flag_1 == 1)
{
R_Left_Turn_Up_Point_Flag_1 = 0;
Get_R_Left_Turn_Up_Point_2(R_Left_Turn_Up_Point_1[1] - 7, R_Left_Turn_Up_Point_1[1] + 7);
{
if(R_Left_Turn_Up_Point_Flag_2 == 1)
{
R_Left_Turn_Up_Point_Flag_2 = 0;
R_Left_Turn_Up_Point_Flag = 1;
R_Left_Turn_Up_Point[0] = R_Left_Turn_Up_Point_1[0];
R_Left_Turn_Up_Point[1] = R_Left_Turn_Up_Point_1[1];
}
}
}
}
4、逆透视求角度找拐点
使用此方法需先求得逆透视变换数组，再使用逆透视求取角度。逆透视实现方法可以参考之前的文章：智能车摄像头开源—5 逆透视处理
//-------------------------------------------------------------------------------------------------------------------
// @brief 逆透视知三点求形成的角度
// @param Ax，Ay 下边点
// @param Bx，By 要求角度的一点
// @param Cx，Cy 上边点
// @return
// @since v1.0
// Sample usage:
// 注：使用时传入原图像的三个点，不可传入逆透视图像的三个点
//-------------------------------------------------------------------------------------------------------------------
float Get_Turn_Point_Angle(uint8 Ax, uint8 Ay, uint8 Bx, uint8 By, uint8 Cx, uint8 Cy)
{
float BA = 0.00;//向量BA的模
float BC = 0.00;
float SBA_BC = 0.00;//向量点乘的值
float Angle = 0.00;
uint8 AX = Back_Inverse_Matrix_Row[Ay * Image_X + Ax];
uint8 AY = Back_Inverse_Matrix_Col[Ay * Image_X + Ax];
uint8 BX = Back_Inverse_Matrix_Row[By * Image_X + Bx];
uint8 BY = Back_Inverse_Matrix_Col[By * Image_X + Bx];
uint8 CX = Back_Inverse_Matrix_Row[Cy * Image_X + Cx];
uint8 CY = Back_Inverse_Matrix_Col[Cy * Image_X + Cx];
BA = sqrt((float)((AX-BX)*(AX-BX)+(AY-BY)*(AY-BY)));
BC = sqrt((float)((CX-BX)*(CX-BX)+(CY-BY)*(CY-BY)));
SBA_BC = (float)((AX-BX)*(CX-BX)+(AY-BY)*(CY-BY));
Angle = acos(SBA_BC * 1.00/ (BA * BC));
return Angle * 57.3f;
}
//逆透视后的图像拐点求角度，传入已经逆透视后的三个点
float I_Get_Turn_Point_Angle(uint8 Ax, uint8 Ay, uint8 Bx, uint8 By, uint8 Cx, uint8 Cy)
{
float BA = 0.00;//向量BA的模
float BC = 0.00;
float SBA_BC = 0.00;//向量点乘的值
float Angle = 0.00;
BA = sqrt((float)((Ax-Bx)*(Ax-Bx)+(Ay-By)*(Ay-By)));
BC = sqrt((float)((Cx-Bx)*(Cx-Bx)+(Cy-By)*(Cy-By)));
SBA_BC = (float)((Ax-Bx)*(Cx-Bx)+(Ay-By)*(Cy-By));
Angle = acos(SBA_BC * 1.00/ (BA * BC));
return Angle * 57.3f;
}寻找时对所有的二维边线点进行遍历寻找。至于三个点的输入顺序，已经知道中间点，其余两个点可以尝试两次即可确定。经本人尝试，此方法并不稳定，所以后续未使用，便不过多赘述，若有兴趣可自行研究。
三、边界点求取
因为使用爬线算法，提前在图像中补了黑框，所以在遇到十字、弯道等图像时，不可避免的会有部分边线落在黑框上，在此我称之为边界点，即位于左右两侧边界上的点。
找出左右两侧边界上的点的个数，可用于判断弯道（一侧有边界点，另一侧没有边界点）、判断十字（左右两侧都有边界点），当然还有环岛等，要根据具体情况去使用，以下给出代码：
/**
* 函数功能： 寻找左侧边线的边界点个数
* 特殊说明： 无
* 形 参： 无
*
* 示例： Get_L_Border_Point_Num();
* 返回值： 无
*/
uint8 L_Border_Point_Num = 0;
uint8 L_UP_Border_Point_Num = 0; //记录位于顶部黑框上的边界点个数
void Get_L_Border_Point_Num(void)
{
uint8 i = 0;
L_Border_Point_Num = 0;
L_UP_Border_Point_Num = 0;
for(i = Y_Meet; i < L_Line[0][1]; i++)
{
if(L_Border[i] == 2) //左右两侧用一维边线即可，2是我的左侧边界
{
L_Border_Point_Num ++;
}
}
for(i = 40; i < L_Statics - 2; i++)
{
if(L_Line[i][1] == 2) //顶部的边界点使用二维边线
{
L_UP_Border_Point_Num ++;
}
}
}
/**
* 函数功能： 寻找右侧边线的边界点个数
* 特殊说明： 无
* 形 参： 无
*
* 示例： Get_R_Border_Point_Num();
* 返回值： 无
*/
uint8 R_Border_Point_Num = 0;
uint8 R_UP_Border_Point_Num = 0;
void Get_R_Border_Point_Num(void)
{
uint8 i = 0;
R_Border_Point_Num = 0;
R_UP_Border_Point_Num = 0;
for(i = Y_Meet; i < R_Line[0][1]; i++)
{
if(R_Border[i] == 77) //77是我的右侧边界
{
R_Border_Point_Num ++;
}
}
for(i = 40; i < R_Statics - 2; i++)
{
if(R_Line[i][1] == 2)
{
R_UP_Border_Point_Num ++;
}
}
}
四、对位边界行
当图像的某一行，它的左侧边线点位于左侧边界上的同时，右侧边线点也位于右侧边界上，那我称之为对位边界行，可以用来辅助判断十字路口，正常情况下只有十字路口才会出现数个对位边界行。
/**
* 函数功能： 寻找对位边界行的个数
* 特殊说明： 无
* 形 参： 无
*
* 示例： Get_Relative_Border_Point_Num();
* 返回值： 无
*/
uint8 Relative_Border_Point_Num = 0; //存储对位边界行的个数
void Get_Relative_Border_Point_Num(void)
{
uint8 i = 0;
Relative_Border_Point_Num = 0; //函数每张图像边线都调用一次，每调用一次都要清零
for(i = Y_Meet; i < L_Start_Point[1]; i++)
{
if(L_Border[i] == 2 && R_Border[i] == 77) //2为左侧边界的X坐标，77为右侧边界
{
Relative_Border_Point_Num ++;
}
}
}五、圆弧拐点求取
圆弧拐点常见于小S弯道以及环岛，可以用于元素判断。判断直角拐点的时候容易与圆弧拐点误判，但有不同的需求场景，具体自己真正熟练使用后就可理解。
圆弧拐点的判断原理为找一段连续点中最突出的那个点，例如左侧内凹的圆弧拐点，其前方的数个点X坐标一直在增大，其之后的数个点X坐标值一直在减小，同时使用生长方向进行辅助判定，生长方向使用的是本人八向迷宫算法中的方向，可查看之前文章去了解：
/**
* 函数功能： 寻找左侧圆弧拐点
* 特殊说明： 无
* 形 参： 无
*
* 示例： Get_L_Arc_Turn_Point();
* 返回值： 无
*/
uint8 L_Arc_Turn_Point[3][2] = {{0}}; //最多找三个就足够使用
uint8 L_Arc_Turn_Point_Flag = 0; //找到一个就挂出标志位
uint8 L_Arc_Turn_Point_Num = 0; //记录找到的个数
void Get_L_Arc_Turn_Point(void)
{
uint8 i = 0;
L_Arc_Turn_Point_Flag = 0;
for(i = 0; i < 3; i++) //每次调用先清零
{
L_Arc_Turn_Point_Num = 0;
L_Arc_Turn_Point[i][0] = 0;
L_Arc_Turn_Point[i][1] = 0;
}
for(i =7; i < (L_Statics - 5); i++)
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 7)))
{
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]) &&
(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]) &&
(L_Grow_Dir[i + 1] == -2 || L_Grow_Dir[i + 1] == 1) && (L_Grow_Dir[i + 3] == -2 || L_Grow_Dir[i + 3] == 1) &&
(L_Grow_Dir[i - 1] == 1 || L_Grow_Dir[i + 1] == 4) && (L_Grow_Dir[i - 3] == 1 || L_Grow_Dir[i - 3] == 4)) //左侧内凹
{
L_Arc_Turn_Point[L_Arc_Turn_Point_Num][0] = L_Line[i][0];
L_Arc_Turn_Point[L_Arc_Turn_Point_Num][1] = L_Line[i][1];
L_Arc_Turn_Point_Flag = 1;
L_Arc_Turn_Point_Num ++;
i += 15; //每个圆弧拐点前后几个点可能都满足圆弧拐点的条件，所以找到后加15个点
if(L_Arc_Turn_Point_Num == 3)
{
break;
}
}
}
}
for(i =7; i < (L_Statics - 5); i++)
{
if((L_Line[i][1] >= (Y_Border_Min + 2)) && (L_Line[i][1] <= (Y_Border_Max - 7)))
{
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]) &&
(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]) &&
(L_Grow_Dir[i + 1] == 4 || L_Grow_Dir[i + 1] == 1) && (L_Grow_Dir[i + 3] == 4 || L_Grow_Dir[i + 3] == 1) &&
(L_Grow_Dir[i - 1] == 1 || L_Grow_Dir[i + 1] == -2) && (L_Grow_Dir[i - 3] == 1 || L_Grow_Dir[i - 3] == -2)) //左侧外凸
{
L_Arc_Turn_Point[L_Arc_Turn_Point_Num][0] = L_Line[i][0];
L_Arc_Turn_Point[L_Arc_Turn_Point_Num][1] = L_Line[i][1];
L_Arc_Turn_Point_Flag = 1;
L_Arc_Turn_Point_Num ++;
i += 15;
if(L_Arc_Turn_Point_Num == 3)
{
break;
}
}
}
}
}
/**
* 函数功能： 寻找右侧圆弧拐点
* 特殊说明： 无
* 形 参： 无
*
* 示例： Get_R_Arc_Turn_Point();
* 返回值： 无
*/
uint8 R_Arc_Turn_Point[3][2] = {{0}};
uint8 R_Arc_Turn_Point_Flag = 0;
uint8 R_Arc_Turn_Point_Num = 0;
void Get_R_Arc_Turn_Point(void)
{
uint8 i = 0;
R_Arc_Turn_Point_Flag = 0;
for(i = 0; i < 3; i++)
{
R_Arc_Turn_Point_Num = 0;
R_Arc_Turn_Point[i][0] = 0;
R_Arc_Turn_Point[i][1] = 0;
}
for(i = 7; i < (R_Statics - 5); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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]) &&
(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]) &&
(R_Grow_Dir[i + 1] == 4 || R_Grow_Dir[i + 1] == 1) && (R_Grow_Dir[i + 3] == 4 || R_Grow_Dir[i + 3] == 1) &&
(R_Grow_Dir[i - 1] == 1 || R_Grow_Dir[i + 1] == -2) && (R_Grow_Dir[i - 3] == 1 || R_Grow_Dir[i - 3] == -2)) //右侧内凹
{
R_Arc_Turn_Point[R_Arc_Turn_Point_Num][0] = R_Line[i][0];
R_Arc_Turn_Point[R_Arc_Turn_Point_Num][1] = R_Line[i][1];
R_Arc_Turn_Point_Flag = 1;
R_Arc_Turn_Point_Num ++;
i += 15;
if(R_Arc_Turn_Point_Num == 3)
{
break;
}
}
}
}
for(i = 7; i < (R_Statics - 5); i++)
{
if((R_Line[i][1] >= (Y_Border_Min + 2)) && (R_Line[i][1] <= (Y_Border_Max - 2)))
{
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]) &&
(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]) &&
(R_Grow_Dir[i + 1] == -2 || R_Grow_Dir[i + 1] == 1) && (R_Grow_Dir[i + 3] == -2 || R_Grow_Dir[i + 3] == 1) &&
(R_Grow_Dir[i - 1] == 1 || R_Grow_Dir[i + 1] == 4) && (R_Grow_Dir[i - 3] == 1 || R_Grow_Dir[i - 3] == 4)) //右侧外凸
{
R_Arc_Turn_Point[R_Arc_Turn_Point_Num][0] = R_Line[i][0];
R_Arc_Turn_Point[R_Arc_Turn_Point_Num][1] = R_Line[i][1];
R_Arc_Turn_Point_Flag = 1;
R_Arc_Turn_Point_Num ++;
i += 15;
if(R_Arc_Turn_Point_Num == 3)
{
break;
}
}
}
}
}
六、求每侧边线斜率及截距
将每侧边线由中间点分割为两段（后称为一段和二段），分别计算一段、二段和整条边线的斜率以及截距（一维边线），然后将三者的斜率进行比对。当一段斜率近似等于二段斜率，一段斜率近似等于整段斜率，二段斜率近似等于整段斜率时，可判定为该侧边线为直线，用于直道的判定。
/**
* 函数功能： 最小二乘法计算斜率
* 特殊说明： 无
* 形 参： uint8 begin 输入起点
* uint8 end 输入终点
* uint8 *border 输入需要计算斜率的一维边线数组
* 示例： Slope_Calculate(start, end, border);
* 返回值： Result 计算出的斜率
*/
float Slope_Calculate(uint8 begin, uint8 end, uint8 *border) //注：begin 必须小于 end,一般 begin 位于图像上方， end 位于图像下方
{
float X_Sum = 0, Y_Sum = 0, XY_Sum = 0, X2_Sum = 0;
int16 i = 0;
float Result = 0;
static float Result_Last;
for(i = begin; i < end ; i++)
{
X_Sum += (float)i;
Y_Sum += (float)border[i];
XY_Sum += (float)i * (border[i]);
X2_Sum += (float)i * i;
}
if((end - begin) * X2_Sum - X_Sum * X_Sum) //防止为0的情况出现
{
Result = ((float)(end - begin) * XY_Sum - X_Sum * Y_Sum) / ((float)(end - begin) * X2_Sum - X_Sum * X_Sum);
Result_Last = Result;
}
else
{
Result = Result_Last;
}
return Result;
}
/**
* 函数功能： 计算斜率截距
* 特殊说明： 调用最小二乘法计算斜率
* 形 参： uint8 start 输入起点
* uint8 end 输入终点
* uint8 *border 输入需要计算斜率的一维边线数组
* float *slope_rate 存储斜率的变量地址
* float *intercept 存储截距的变量地址
* 示例： Calculate_Slope_Intercept(start, end, L_Border, &L_Straightaway_Lope_Rate_C, &L_Intercept);
* 返回值： 无
*/
void Calculate_Slope_Intercept(uint8 start, uint8 end, uint8 *border, float *slope_rate, float *intercept)
{
uint16 i, Num = 0;
uint16 X_Sum = 0, Y_Sum = 0;
float Y_Average = 0, X_Average = 0;
for(i = start; i < end; i++)
{
X_Sum += i;
Y_Sum += border[i];
Num ++;
}
if(Num)
{
X_Average = (float)(X_Sum / Num);
Y_Average = (float)(Y_Sum / Num);
}
*slope_rate = Slope_Calculate(start, end, border);
*intercept = (float)(Y_Average - (*slope_rate) * X_Average);
}
float Max_Slope_Dif_Thre = 0.6; //实测得出，并未使用
float Min_Slope_Dif_Thre = 0.15; //实测得出，当两段线段的斜率低于这个阈值时，可认为斜率相等
//斜率和截距
float R_Straightaway_Lope_Rate_A = 0; //第一段的斜率
float R_Straightaway_Lope_Rate_B = 0; //第二段的斜率
float R_Straightaway_Lope_Rate_C = 0; //整段边线的斜率
float R_Intercept = 0; //整段边线的截距
uint8 R_Straight_Flag = 0; //右侧边线是否为直线标志位
/**
* 函数功能： 求右侧边线斜率截距，并判断是否为直线
* 特殊说明： 无
* 形 参： uint8 start //右侧边线起始点Y坐标
* uint8 end //右侧边线终止点Y坐标
*
* 示例： Get_R_Intercept_And_Slope(R_Line[R_Statics][1] + 5, R_Line[0][1]);
* 返回值： 无
*/
void Get_R_Intercept_And_Slope(uint8 start, uint8 end)
{
R_Straightaway_Lope_Rate_A = 0;
R_Straightaway_Lope_Rate_B = 0;
R_Straightaway_Lope_Rate_C = 0;
R_Intercept = 0;
R_Straight_Flag = 0; //所有值先清零
R_Straightaway_Lope_Rate_A = Slope_Calculate(start, ((end - start) / 2) + start, R_Border);
R_Straightaway_Lope_Rate_B = Slope_Calculate(((end - start) / 2) + start, end, R_Border); //计算两段斜率
Calculate_Slope_Intercept(start, end, R_Border, &R_Straightaway_Lope_Rate_C, &R_Intercept); //计算整段边线斜率和截距
if(My_ABS_F(R_Straightaway_Lope_Rate_A - R_Straightaway_Lope_Rate_B) <= Min_Slope_Dif_Thre &&
My_ABS_F(R_Straightaway_Lope_Rate_B - R_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre &&
My_ABS_F(R_Straightaway_Lope_Rate_A - R_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre) //判断是否为直线
{
R_Straight_Flag = 1;
}
}
float L_Straightaway_Lope_Rate_A = 0;
float L_Straightaway_Lope_Rate_B = 0;
float L_Straightaway_Lope_Rate_C = 0;
float L_Intercept = 0;
uint8 L_Straight_Flag = 0;
/**
* 函数功能： 求左侧边线斜率截距，并判断是否为直线
* 特殊说明： 无
* 形 参： uint8 start //左侧边线起始点Y坐标
* uint8 end //左侧边线终止点Y坐标
*
* 示例： Get_L_Intercept_And_Slope(L_Line[L_Statics][1] + 5, L_Line[0][1]);
* 返回值： 无
*/
void Get_L_Intercept_And_Slope(uint8 start, uint8 end)
{
L_Straightaway_Lope_Rate_A = 0;
L_Straightaway_Lope_Rate_B = 0;
L_Straightaway_Lope_Rate_C = 0;
L_Intercept = 0;
L_Straight_Flag = 0;
L_Straightaway_Lope_Rate_A = Slope_Calculate(start, ((end - start) / 2) + start, L_Border);
L_Straightaway_Lope_Rate_B = Slope_Calculate(((end - start) / 2) + start, end, L_Border);
Calculate_Slope_Intercept(start, end, L_Border, &L_Straightaway_Lope_Rate_C, &L_Intercept);
if(My_ABS_F(L_Straightaway_Lope_Rate_A - L_Straightaway_Lope_Rate_B) <= Min_Slope_Dif_Thre &&
My_ABS_F(L_Straightaway_Lope_Rate_B - L_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre &&
My_ABS_F(L_Straightaway_Lope_Rate_A - L_Straightaway_Lope_Rate_C) <= Min_Slope_Dif_Thre)
{
L_Straight_Flag = 1;
}
}
七、拟合曲线计算方差
当我们求出左右两侧一维边线的斜率和截距以后，可以用斜率和截距在图像中画出一条直线，然后将实际的赛道边线的每个点（一维边线）与拟合直线的对应行的点计算方差，整条边线的方差的大小反应了赛道边线的弯曲程度。
但实际验证此方法时，效果不甚理想，会遇到各种特殊情况，同时以下代码存在些许bug，作者到最终都没修复，所以更多是在此处给出一个思路，想用的话可以尝试修修下列代码的bug：
//拟合曲线
int16 L_Fitting_Line[] = {0};
int16 R_Fitting_Line[] = {0};
//方差
float L_Variance = 0;
float R_Variance = 0;
/**
* 函数功能： 计算左右两侧边线的方差
* 特殊说明： 每三行计算一次，减少计算量，但获取的信息不那么精确，可以选择计算每一行
* 形 参： 无
*
* 示例： Get_Fitting_Line_And_Variance();
* 返回值： 无
*/
void Get_Fitting_Line_And_Variance(void)
{
uint8 i = 0;
for(i = 1; i <= 19; i ++)
{
L_Fitting_Line[i] = (int16)(L_Straightaway_Lope_Rate_C * (float)(i * 3) + L_Intercept); //y = kx+b(把XY轴交换下就能想通了)
L_Fitting_Line[i] = Limit_16(L_Fitting_Line[i], X_Border_Min + 2, X_Border_Max - 2);
R_Fitting_Line[i] = (int16)(R_Straightaway_Lope_Rate_C * (float)(i * 3) + R_Intercept);
R_Fitting_Line[i] = Limit_16(R_Fitting_Line[i], X_Border_Min + 2, X_Border_Max - 2);
}
Get_Variance(L_Line[L_Statics][1] + 1, L_Line[0][1], L_Fitting_Line, L_Border, &L_Variance, 3);
Get_Variance(R_Line[R_Statics][1] + 1, R_Line[0][1], R_Fitting_Line, R_Border, &R_Variance, 3);
}八、求取左右侧边线差值
用每行右侧边线点的X坐标值减去对应行左侧边线点的X坐标值（一维边线），可以得到每行的差值（或者说每行赛道的宽度），由此可以用来判断十字等元素。例如十字：图像从下往上的边线差值先变小，后突然变大，后又变小，呈现出十字的趋势。
以下代码每隔两行计算一行，并没有计算全部行的差值，但是计算每一行完全可行。
int8 Row_Difference[20] = {0};
uint8 Max_Row_Dif_Line_Num = 0; //当某一行左侧边线点位于左侧边界上，右侧边线点位于右侧边线上，与对位边界行同理，可以记录下来
/**
* 函数功能： 寻找对位边界行的个数
* 特殊说明： 无
* 形 参： 无
*
* 示例： Get_Relative_Border_Point_Num();
* 返回值： 无
*/
void Get_Row_Difference(uint8 *l_border, uint8 *r_border)
{
uint8 i = 0;
Max_Row_Dif_Line_Num = 0;
for(i = 0; i < 20; i ++)
{
Row_Difference[i] = (int8)(r_border[i * 3] - l_border[i * 3]);
if(Row_Difference[i] == 75)
{
Max_Row_Dif_Line_Num ++;
}
}
}九、TOF测距
部分组别使用TOF测距模块检测障碍、坡道等元素，那么就需要调用对应函数获取距离。逐飞官方手册中说明：测量间隔不可低于10ms。因此我们直接使用图像计数（采集十张图像间隔绝对大于10ms，并且时间相对固定，可根据自己帧率适当调整）
if(Image_Num % 10 == 0) //其中Image_Num 需在复制图像时加一，即没采集一张图像加一一次
{
Barrier_Distance = TOF_Get_Distance_mm();
}
十、总函数调用
将上述所有处理进行整合，放至一个函数内，每张图像调用一次函数便可获取所有信息。对于部分元素内部情况（例如环岛、十字等特殊元素会被对应的循环接管），只需调用对应的函数获取所需的信息即可。顺序可随意
/**
* 函数功能： 获取所有信息
* 特殊说明： 无
* 形 参： 无
*
* 示例： Get_Information();
* 返回值： 无
*/
void Get_Information(void)
{
Get_L_Border_Point_Num();
Get_R_Border_Point_Num();
Get_Relative_Border_Point_Num();
Get_L_Up_Turn_Left_Point_1(2);
Get_L_Right_Turn_Up_Point_1(2);
Get_R_Up_Turn_Right_Point_1(2);
Get_R_Left_Turn_Up_Point_1(2);
Get_L_Arc_Turn_Point();
Get_R_Arc_Turn_Point();
Get_L_Intercept_And_Slope(L_Line[L_Statics][1] + 5, L_Line[0][1]);
Get_R_Intercept_And_Slope(R_Line[R_Statics][1] + 5, R_Line[0][1]);
Get_Fitting_Line_And_Variance();
Get_Row_Difference(L_Border, R_Border);
}十一、总结
以上处理后就完成了对于边线信息的提取，加上之前的处理，我们可以获取一张图像赛道的以下信息：
1、左右两侧边线的二维数组
- 每个点的X，Y坐标；
- 每个点的生长方向；
- 每个点的阈值；
2、每侧二维边线点的个数
3、左右两侧边线的一维数组
- 每个点的X，Y（数组下标）坐标；
4、每侧一维边线点的个数
5、所有二维边线点的均值阈值
6、左右两侧爬线相遇点的X，Y坐标
7、左右两侧边界点的个数
8、顶部边界点的个数
9、对位边界行的个数
10、上转左、上转右、右转上、左转上四个拐点坐标及标志位
11、每侧最多三个圆弧拐点及标志位
12、每侧边线的斜率、截距以及是否为直线标志位
13、每侧边线的方差（弯曲程度）
14、左右两侧边线的差值
15、障碍物（坡道）距离
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
