一、前言
本篇将详细讲述自适应八向迷宫的算法原理，优势以及弊端。
同样在此声明：此系列开源均由本人实践和经验得出，并不保证完全正确，仅供参考和入门学习。
二、自适应八向迷宫优势
- 具备极快的速度优势，在双核主频200MHz英飞凌TC264主控上，单核运算一张180 × 120（包含压缩）图像得出二维和一维边线耗时在0.2-0.3ms之间，完全不用担心主控资源占用问题。
- 直接处理灰度图像，不需对整张图像进行二值化。
- 当找到起始点后，只锁定赛道边线进行爬线，配合局部阈值，使赛道边线附近5×5以外的像素均不参与运算，大幅节省算力的同时，避免了蓝布上杂物的干扰。
- 每个点均由局部阈值得出，对明暗不均的光线情况表现出良好的适应性。
- 超快的速度优势，可以使摄像头（我们使用的是逐飞总钻风130º无畸变摄像头）跑满500帧（上限），即2ms输出一次偏差值，可使控制更加细腻，偏差值波形极度平滑，而在摩托实际运行中，100帧与500帧，表现出肉眼可见的差异。
- 在上交开源代码原基础上引入方向解算，对每个边线点的计算量进行优化，进一步减少计算量的同时，保留每个点的生长方向，可用于后续元素判断和元素处理。
- 相同曝光时间和曝光增益下，关闭自动曝光时，更高的帧率得到的图像会更暗，即在应对非常亮的赛道环境时，具备更高的上限。
三、自适应八向迷宫弊端
- 若寻起点的图像行出现难以处理的干扰，会导致整个边线的紊乱，即本张图像提取的信息不具备可信度。这种情况常见于：连接两个赛道之间的白色胶带反光能力过强，导致图像底层出现极小的高亮亮斑；赛道过脏，导致图像底部出现大量暗斑；
- 直接处理灰度图像，而没有二值化，不能保证每张图像边线提取的稳定性，需要更多后续的滤波算法。同时会增加后续元素处理的难度。
- 因其特性，导致补线处理元素几乎不可能，但有单边巡线的方法来针对性解决此类问题，后续文案会给出。
- 对于平衡组如摩托这种，过弯会使摄像头倾斜，此算法会导致元素处理难度急剧增加，所以如果后续出了类似于摩托这种既需平衡，也会导致摄像头倾斜的组别，建议果断放弃。但对于常规竞速组别，此算法绝对可以提高上限，需读者自行斟酌。
这里再补充一些对二值化图像的看法：
当光线均匀时，使用大津法二值化图像，会将图像分为黑白分明的几大块，那么求边线时，黑就是黑，白就是白，会使求取的边线十分稳定。当出现局部高亮反光时，大津法可能会凹下去一部分，但边线仍不至于说受到毁灭性打击。
同时说起Sobel提取边缘，最终得到的二值化图像应该包含两道连续的黑线（其他外界的黑线不算在内），即赛道边线，但出现局部高反光时，会使黑线出现断裂，那么如果使用爬线算法，就会直接爬乱掉。对于诸如Canny等的算法也存在同样问题。
那么就不得不引出对于爬线算法的讨论，爬线算法可以得出二维数组边线，并根据算法同时得出一维数组边线，那么提取信息量是很大的，同时速度也非常快。但是，由于爬线算法是根据上一个点的位置引出下一个点，那么只要几个点出现混乱，大概率会导致后面的边线也出现混乱，使一张图像的信息不可信。
这时候，使用最长白直列向左右两侧扫线仿佛也不再那么low，它针对性提取一维边线，且每个边线点不再相互关联，一个乱了不影响后续的扫线。
但根据我的比赛经历，华东赛区和国赛并没有出现光线很差的情况，所以大可放心使用爬线算法，当然也可写出扫线算法，以备不时之需。
四、自适应八向迷宫详解
下面开始干货。算法整体比较冗长，可能会使刚接触到的读者毫无头绪，看起来混乱如麻，但实际上正如其名，可将算法拆分为多个部分分开理解，并最后整合，会有一种豁然开朗的感觉。下面分四部分讲解：自适应阈值、迷宫巡线、方向解算、方向优化阈值计算。
1.自适应阈值
自适应阈值简单易懂，将一个5 * 5区域内的所有灰度值相加，再除 25 就是此区域内的局部阈值，至于为什么自适应，因为每个边线点的阈值都是由其与周围24个点的灰度值求均值得到，可有效适应图像不同区间明暗不均的情况，不同于大津的全局阈值，适应性大大增强。
这里给出求阈值部分的分代码（对一个中心点求阈值）：
const int8 Square_0[25][2] = { //一个5 * 5的矩阵，用来求中心点周围的局部阈值（局部阈值）
{-2,-2},{-1,-2},{0,-2},{+1,-2},{+2,-2},
{-2,-1},{-1,-1},{0,-1},{+1,-1},{+2,-1},
{-2,-0},{-1, 0},{0, 0},{+1, 0},{+2,-0},
{-2,+1},{-1,+1},{0,+1},{+1,+1},{+2,+1},
{-2,+2},{-1,+2},{0,+2},{+1,+2},{+2,+2}
};
for (j = 0; j < 25; j++) //计算阈值和
{
L_Pixel_Value_Sum += image[L_Center_Point[1] + Square_0[j][1]][L_Center_Point[0] + Square_0[j][0]];
}
L_Thres = (L_Pixel_Value_Sum + Thres_Interfere) / Thres_Num_Interfere; //阈值为25个点灰度值的平均值
L_Thres -= clip_value; //将得到的灰度阈值减去一个经验值，用来优化判定
备注：Thres_Interfere：为手动干预阈值和，可以在二十五个值的和上进行修改。但最终未使用。
Thres_Num_Interfere：点的个数，这里取25即可
clip_value：经验值参数，上交代码中引用此参数用于微调阈值，防止强行分割，取值为0 ~ 5即可，
但最终本人未使用2.迷宫巡线
首先要清楚赛道分为左右两侧边线，因此爬线需分别处理左侧边线和右侧边线。
迷宫算法最关键的一点是对面向的把控。对于一个图像中一个点来说，它可以有八个方向：上、下、左、右、左上、左下、右上、右下。当清楚这一点后，便将自己带入迷宫算法，假设自己站在这个点上，那么自己的面向会有以上八个方向。但对于迷宫爬线来说，只需用到上下左右四个面向。可参照如下草图中的框架（其中，灰度值小于阈值为黑，大于阈值为白）配合手绘图像（在此我们假设有一张二值化后的黑白图像）来理解：
如上图所示，假设起点已知，迷宫爬线初始时起点面向于上方，参照框架流程：面向为黑，原地向右转90°—>面向为白（此时面向向右）—>判断此时面向左前方为白—>向左前方走一次并向左转90°。
后续都参照框架流程，就可以得到图中所有的圆圈，即边线点。建议多走几次，将算法原理理解透彻。然后右侧同理：
当将迷宫巡线的算法原理理解透彻并记牢后，就可以读代码了：（应该蛮简单的吧，加油少年！！！！！）
const int8 L_Face_Dir[4][2] = {{0,-1},{1,0},{0,1},{-1,0}}; //左侧迷宫面向
// 0
//3 1
// 2
const int8 L_Face_Dir_L[4][2] = {{-1,-1},{1,-1},{1,1},{-1,1}}; //左侧面向的左前方
//0 1
//
//3 2
const int8 R_Face_Dir[4][2] = {{0,-1},{1,0},{0,1},{-1,0}}; //右侧迷宫面向
// 0
//3 1
// 2
const int8 R_Face_Dir_R[4][2] = {{1,-1},{1,1},{-1,1},{-1,-1}}; //右侧面向的右前方
//3 0
//
//2 1
L_Stop_Flag = 0; //左侧停止爬线标志位，用于死区处理
R_Stop_Flag = 0; //右侧停止爬线标志位，用于死区处理
//左边变量
uint8 L_Center_Point[2] = { 0 }; //存放每次找到的XY坐标
uint16 L_Data_Statics = 0; //统计左边找到的边线点的个数
uint8 L_Front_Value = 0; //左侧 面向的前方点的灰度值
uint8 L_Front_L_Value = 0; //左侧 面向的左前方点的灰度值
uint8 L_Dir = 0; //此参数用于转向
uint8 L_Turn_Num = 0; //记录转向次数，若中心点前后左右都是黑色像素，就会在一个点转向四次，记录到四次时退出循环防止卡死程序
uint16 L_Pixel_Value_Sum = 0; //中心点与周围24个点的像素值和
float L_Thres = 0; //局部阈值,即L_Pixel_Value_Sum / 25
//右边变量
uint8 R_Center_Point[2] = { 0 }; //存放每次找到的XY坐标
uint16 R_Data_Statics = 0; //统计右边找到的边线点的个数
uint8 R_Front_Value = 0; //右侧 面向的前方点的灰度值
uint8 R_Front_R_Value = 0; //右侧 面向的左前方点的灰度值
uint8 R_Dir = 0; //此参数用于转向
uint8 R_Turn_Num = 0; //记录转向次数，若中心点前后左右都是黑色像素，就会在一个点转向四次，记录到四次时退出循环防止卡死程序
uint16 R_Pixel_Value_Sum = 0; //中心点与周围24个点的像素值和
float R_Thres = 0; //局部阈值
while(L_Stop_Flag == 0 && R_Stop_Flag == 0)
{
//求左侧当前5 * 5区域的阈值，这里省去代码
L_Data_Statics++;
L_Judge_Again: //L_Judge_Again 与 goto 配合使用
if (L_Stop_Flag == 0)
{
//L_Center_Point为当前左侧5 * 5区域的中心点
L_Front_Value = image[L_Center_Point[1] + L_Face_Dir[L_Dir][1]][L_Center_Point[0] + L_Face_Dir[L_Dir][0]]; //记录面向的前方点的灰度值
L_Front_L_Value = image[L_Center_Point[1] + L_Face_Dir_L[L_Dir][1]][L_Center_Point[0] + L_Face_Dir_L[L_Dir][0]]; //记录面向的左前方点的灰度值
if ((float)L_Front_Value < L_Thres) //面向的前方点是黑色
{
L_Dir = (L_Dir + 1) % 4; //需右转一次
L_Turn_Num++;
if (L_Turn_Num == 4) //死区处理
{
L_Stop_Flag = 1; //当前后左右都是黑色时，进入死区，停止左侧爬线
}
goto L_Judge_Again;
}
else if ((float)L_Front_L_Value < L_Thres) //左前方点是黑色，前方点是白色
{
L_Center_Point[0] += L_Face_Dir[L_Dir][0];
L_Center_Point[1] += L_Face_Dir[L_Dir][1]; //向前走一步
l_dir[L_Data_Statics - 1] = (L_Face_Dir[L_Dir][0] * 3) - L_Face_Dir[L_Dir][1]; //这里是方向解算，下面再讲解，可略过这行代码
L_Turn_Num = 0;
}
else //左前方和前方都是白色点
{
L_Center_Point[0] += L_Face_Dir_L[L_Dir][0];
L_Center_Point[1] += L_Face_Dir_L[L_Dir][1]; //向左前方走一步
l_dir[L_Data_Statics - 1] = (L_Face_Dir_L[L_Dir][0] * 3) - L_Face_Dir_L[L_Dir][1]; //这里是方向解算，下面再讲解，可略过这行代码
L_Dir = (L_Dir + 3) % 4; //左转一次
L_Turn_Num = 0;
}
if (L_Data_Statics >= 4) //O环处理，即转了一圈后回到原处，也是一种死区，当立即停止爬线
{
if (l_line[L_Data_Statics][0] == l_line[L_Data_Statics - 4][0] &&
l_line[L_Data_Statics][1] == l_line[L_Data_Statics - 4][1])
{
L_Stop_Flag = 1;
}
}
}
//求右侧当前5 * 5区域的阈值，这里省去代码
R_Data_Statics++;
R_Judgme_Again:
if (R_Stop_Flag == 0)
{
R_Front_Value = image[R_Center_Point[1] + R_Face_Dir[R_Dir][1]][R_Center_Point[0] + R_Face_Dir[R_Dir][0]];
R_Front_R_Value = image[R_Center_Point[1] + R_Face_Dir_R[R_Dir][1]][R_Center_Point[0] + R_Face_Dir_R[R_Dir][0]];
if ((float)R_Front_Value < R_Thres)
{
R_Dir = (R_Dir + 3) % 4;
R_Turn_Num++;
if (R_Turn_Num == 4)
{
R_Stop_Flag = 1;
}
goto R_Judgme_Again;
}
else if ((float)R_Front_R_Value < R_Thres)
{
R_Center_Point[0] += R_Face_Dir[R_Dir][0];
R_Center_Point[1] += R_Face_Dir[R_Dir][1];
r_dir[R_Data_Statics - 1] = R_Face_Dir[R_Dir][0] * 3 - R_Face_Dir[R_Dir][1];
R_Turn_Num = 0;
}
else
{
R_Center_Point[0] += R_Face_Dir_R[R_Dir][0];
R_Center_Point[1] += R_Face_Dir_R[R_Dir][1];
r_dir[R_Data_Statics - 1] = R_Face_Dir_R[R_Dir][0] * 3 - R_Face_Dir_R[R_Dir][1];
R_Dir = (R_Dir + 1) % 4;
R_Turn_Num = 0;
}
if (R_Data_Statics >= 4)
{
if (r_line[R_Data_Statics][0] == r_line[R_Data_Statics - 4][0] &&
r_line[R_Data_Statics][1] == r_line[R_Data_Statics - 4][1])
{
R_Stop_Flag = 1;
}
}
}
}3、方向解算
之后就是方向解算了，想必读懂迷宫爬线后，就会发现这种爬线方式非常高效，但其并无法像八邻域爬线那样得到生长方向信息。对于这一弊端，我将迷宫的快速性与八邻域的方向性相结合，八向迷宫由此而来。而这里的重点，便是对于方向的解算：
首先浅谈八邻域爬线：八邻域其实很简单，就是中心点的周围有八个点，与迷宫算法类似，八邻域爬线时中心点也是有八个生长方向：上、下、左、右、左上、左下、右上、右下。其八个生长方向被提前定义为0~7的数字，如下：
//八邻域的方向：（左侧为逆时针、右侧为顺时针）
//3 2 1 1 2 3
//4 0 0 4
//5 6 7 7 6 5求取边线时，围绕中心点，由0—>1，1—>2，2—>3……的顺序检测由白到黑的跳变（假设有一张二值化后的图像并且已知起点），当出现跳变时，将此时的白点（也可为黑点，但如果开始记的是黑点，那么就一直记黑点，白点同理）记为下一次的中心点，并将白点所在的数字记为本次中心点的生长方向（爬线算法又名生长算法）。循环上述流程直至得出整个左侧边线，此时会已知每个边线点的坐标和生长方向。这里讲的比较笼统，想详细了解八邻域算法的可以自行搜索，也是很简单，而且算法已经很成熟。
而迷宫算法本身只可得到每个边线点的坐标，要想实现八邻域的效果，也得到每个边线点的生长方向，就得特殊处理。首先我们已知周围八个点与本次中心点的坐标关系（下面称为关系坐标）：
//{-1,-1},{0,-1},{+1,-1},
//{-1, 0}, {+1, 0},
//{-1,+1},{0,+1},{+1,+1},将每个关系横坐标乘 3：
//{-3,-1},{0,-1},{+3,-1},
//{-3, 0}, {+3, 0},
//{-3,+1},{0,+1},{+3,+1},再将关系横坐标减去关系纵坐标：
// -2, 1, 4
// -3, 3
// -4,-1, 2
//此算法无任何原理，只是为了得到八个不一样的值可以用来判定方向，横坐标可以乘大于 2 的任意值，2以内会出现重复由此我们就可以像八邻域一样解算方向了：
const int8 L_Face_Dir[4][2] = {{0,-1},{1,0},{0,1},{-1,0}}; //左侧迷宫面向
// 0
//3 1
// 2
const int8 L_Face_Dir_L[4][2] = {{-1,-1},{1,-1},{1,1},{-1,1}}; //左侧面向的左前方
//0 1
//
//3 2
const int8 R_Face_Dir[4][2] = {{0,-1},{1,0},{0,1},{-1,0}}; //右侧迷宫面向
// 0
//3 1
// 2
const int8 R_Face_Dir_R[4][2] = {{1,-1},{1,1},{-1,1},{-1,-1}}; //右侧面向的右前方
//3 0
//
//2 1
//左侧向面向前方生长时：
//计算下一次中心点的坐标：
L_Center_Point[0] += L_Face_Dir[L_Dir][0];
L_Center_Point[1] += L_Face_Dir[L_Dir][1];
//解算方向：（不必纠结于L_Data_Statics - 1是哪一次的中心点，先理解方向解算）
l_dir[L_Data_Statics - 1] = (L_Face_Dir[L_Dir][0] * 3) - L_Face_Dir[L_Dir][1]; //l_dir为记录方向的数组 L_Dir取值为0， 1， 2， 3，为面向方向
//左侧向面向方向左前方生长时：
L_Center_Point[0] += L_Face_Dir_L[L_Dir][0];
L_Center_Point[1] += L_Face_Dir_L[L_Dir][1];
l_dir[L_Data_Statics - 1] = (L_Face_Dir_L[L_Dir][0] * 3) - L_Face_Dir_L[L_Dir][1];
R_Center_Point[0] += R_Face_Dir[R_Dir][0];
R_Center_Point[1] += R_Face_Dir[R_Dir][1];
//右侧向面向方向生长时：
r_dir[R_Data_Statics - 1] = R_Face_Dir[R_Dir][0] * 3 - R_Face_Dir[R_Dir][1];
//右侧向面向方向右前方生长时：
r_dir[R_Data_Statics - 1] = R_Face_Dir_R[R_Dir][0] * 3 - R_Face_Dir_R[R_Dir][1];
虽不如八邻域的0-7数字那般清晰明了，但熟练掌握后这八个方向数字就会刻在脑海里。
4.方向优化阈值解算
首先要吃透生长方向的原理，然后再来看这部分。直接上图：
拿图一来举例，①代表上次中心点，我们已计算过其周围二十五个点的灰度值和（设为A），②代表本次中心点。当上次中心点向右上方生长时，会发现其计算阈值的5 * 5矩阵有4 * 4的重叠区域，并有9个要抛弃的点和9个新加入的点，那我们直接使用上次中心点的灰度值和A减去九个抛弃点，加上九个新点，由此原本的二十五次加法计算就简化为十八次运算。对于上下左右生长，会出现4 * 5的重叠区域，那么同理，可将二十五次计算简化为十次计算。对于一次阈值计算可能不算什么，但处理整张图像时，可大幅减少运算量，进而提高算法速度。这也是方向解算的另一大优势之一，可以进一步优化阈值计算。
是不是感觉很简单，然后上代码：
//左侧：
switch (l_dir[L_Data_Statics - 1]) //l_dir[L_Data_Statics - 1]为上一个中心点 的生长方向，八个数字代表的生长方向在上面已经讲清楚
{
//从第二个点开始，第一个点（起点）的阈值要25个点全部加一遍
//当横向或纵向生长时，将原先的25次加法运算简化为十次计算
//当斜向生长时，将原先的25次加法计算简化为十六次运算
//可以画出图来，跟着代码走几遍，就可以很快速的理解
case 1:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 3][L_Center_Point[0] + 2] - image[L_Center_Point[1] + 3][L_Center_Point[0] + 1]
- image[L_Center_Point[1] + 3][L_Center_Point[0] + 0] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 1]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] - 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2];
break;
}
case -2:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 1][L_Center_Point[0] + 3] - image[L_Center_Point[1] - 0][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 1][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 2][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 3][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 3][L_Center_Point[0] + 2]
- image[L_Center_Point[1] + 3][L_Center_Point[0] + 1] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 0]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 0][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 0] + image[L_Center_Point[1] - 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2];
break;
}
case -3:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 2][L_Center_Point[0] + 3] - image[L_Center_Point[1] - 1][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 0][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 1][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 2][L_Center_Point[0] + 3]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] - 0][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2];
break;
}
case -4:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 3][L_Center_Point[0] - 1] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 0]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 1] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 2]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 3] - image[L_Center_Point[1] - 2][L_Center_Point[0] + 3]
- image[L_Center_Point[1] - 1][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 0][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 1][L_Center_Point[0] + 3]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 0][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 0] + image[L_Center_Point[1] + 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2];
break;
}
case -1:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 3][L_Center_Point[0] - 2] - image[L_Center_Point[1] - 3][L_Center_Point[0] - 1]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 0] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 1]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] + 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2];
break;
}
case 2:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 1][L_Center_Point[0] - 3] - image[L_Center_Point[1] + 0][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 1][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 2][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 3][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 3][L_Center_Point[0] - 2]
- image[L_Center_Point[1] - 3][L_Center_Point[0] - 1] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 0]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 0][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] + 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2];
break;
}
case 3:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 2][L_Center_Point[0] - 3] - image[L_Center_Point[1] + 1][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 0][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 1][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 2][L_Center_Point[0] - 3]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] + 0][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2];
break;
}
case 4:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 3][L_Center_Point[0] + 1] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 0]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 1] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 2]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 3] - image[L_Center_Point[1] + 2][L_Center_Point[0] - 3]
- image[L_Center_Point[1] + 1][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 0][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 1][L_Center_Point[0] - 3]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 0][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] - 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2];
break;
}
}
//右侧：
switch(r_dir[R_Data_Statics - 1])
{
case 1:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 3][R_Center_Point[0] + 2] - image[R_Center_Point[1] + 3][R_Center_Point[0] + 1]
- image[R_Center_Point[1] + 3][R_Center_Point[0] + 0] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 1]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] - 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2];
break;
}
case -2:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 1][R_Center_Point[0] + 3] - image[R_Center_Point[1] - 0][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 1][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 2][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 3][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 3][R_Center_Point[0] + 2]
- image[R_Center_Point[1] + 3][R_Center_Point[0] + 1] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 0]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 0][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 0] + image[R_Center_Point[1] - 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2];
break;
}
case -3:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 2][R_Center_Point[0] + 3] - image[R_Center_Point[1] - 1][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 0][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 1][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 2][R_Center_Point[0] + 3]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] - 0][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2];
break;
}
case -4:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 3][R_Center_Point[0] - 1] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 0]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 1] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 2]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 3] - image[R_Center_Point[1] - 2][R_Center_Point[0] + 3]
- image[R_Center_Point[1] - 1][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 0][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 1][R_Center_Point[0] + 3]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 0][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 0] + image[R_Center_Point[1] + 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2];
break;
}
case -1:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 3][R_Center_Point[0] - 2] - image[R_Center_Point[1] - 3][R_Center_Point[0] - 1]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 0] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 1]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] + 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2];
break;
}
case 2:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 1][R_Center_Point[0] - 3] - image[R_Center_Point[1] + 0][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 1][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 2][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 3][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 3][R_Center_Point[0] - 2]
- image[R_Center_Point[1] - 3][R_Center_Point[0] - 1] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 0]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 0][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] + 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2];
break;
}
case 3:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 2][R_Center_Point[0] - 3] - image[R_Center_Point[1] + 1][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 0][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 1][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 2][R_Center_Point[0] - 3]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] + 0][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2];
break;
}
case 4:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 3][R_Center_Point[0] + 1] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 0]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 1] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 2]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 3] - image[R_Center_Point[1] + 2][R_Center_Point[0] - 3]
- image[R_Center_Point[1] + 1][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 0][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 1][R_Center_Point[0] - 3]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 0][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] - 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2];
break;
}
}五、额外补充
对函数内的一些内容补充讲解。
1.反向平滑滤波
//反向平滑滤波（不知网上是否有这种算法，此处为本人原创），用于适应由较暗到（局部）高亮区域，或反之的光线情况。
if(Thres_Filiter_Flag_1 == 1 || Thres_Filiter_Flag_2 == 1)
{
if(L_Data_Statics > 3)
{
L_Thres = L_Thres * 1.3f - L_Thres_Record[L_Data_Statics - 1] * 0.2f - L_Thres_Record[L_Data_Statics - 2] * 0.1f;
}
}正常情况下，滤波采用 （a * 上上次值 + b * 上次值 + c * 本次值），其中（a + b + c）的值为1。但由高亮区域突然进入暗区域时，图像会在某一区域内出现灰度值的快速降低，此时若用正常滤波，上上次值 与 上次值 都比 本次值 要大，会导致本次值偏大，即阈值偏大，进而使整个较暗区域全部判定位黑色，无法分割出赛道边线。但反过来滤波，使用公式：（a * 本次值 - b * 上次值 - c * 上上次值），其中（a - b - c）的值为1，那么会把较暗区域的阈值再拉低，进而在较暗区域内分割出边线。由暗区域进入高亮区域也是同理，会使阈值再拉高。可能不太好理解，可以试着画出方块图像，模拟光线剧烈变化时的灰度值，然后尝试用算法求解边线。
但此时好像并不能再称为滤波算法，因其抗干扰能力并不很强，更像是一种应对明暗不均图像的手段。
2、阈值记录
将左右两侧边线的每个点的阈值进行记录，可实现图像迭代：
L_Thres_Record[L_Data_Statics] = L_Thres;
R_Thres_Record[R_Data_Statics] = R_Thres;3、爬线停止
左右两侧同时爬线，每次每侧只求解一个边线点，那如何让两侧爬线停止呢，自然是判定两侧边线的相遇，即两侧每求出一个边线点，就判定最新边线点的X，Y坐标关系，若两侧点相遇（即两边最新点X，Y坐标的差值很小），即停止爬线，同时记录相遇点的坐标，一张图像爬线结束。
if(L_Stop_Flag == 0 && R_Stop_Flag == 0)
{
if ((My_ABS(r_line[R_Data_Statics - 1][0] - l_line[L_Data_Statics - 1][0]) <= 1)
&& (My_ABS(r_line[R_Data_Statics - 1][1] - l_line[L_Data_Statics - 1][1]) <= 1)) //两侧爬线相遇，退出循环，一张图像爬线结束
{
*y_meet = (r_line[R_Data_Statics - 1][1] + l_line[L_Data_Statics - 1][1]) >> 1; //记录相遇点Y
*x_meet = (r_line[R_Data_Statics - 1][0] + l_line[L_Data_Statics - 1][0]) >> 1; //记录相遇点X
break;
}
}
//有一侧存在死区时，对相遇点的判定放宽松一些，防止实际相遇但没有判定出，导致爬线紊乱的情况
else
{
if ((My_ABS(r_line[R_Data_Statics - 1][0] - l_line[L_Data_Statics - 1][0]) <= 3)
&& (My_ABS(r_line[R_Data_Statics - 1][1] - l_line[L_Data_Statics - 1][1]) <= 3)) //两侧爬线相遇，退出循环，一张图像爬线结束
{
*y_meet = (r_line[R_Data_Statics - 1][1] + l_line[L_Data_Statics - 1][1]) >> 1; //记录相遇点Y
*x_meet = (r_line[R_Data_Statics - 1][0] + l_line[L_Data_Statics - 1][0]) >> 1; //记录相遇点X
break;
}
}4、死区（O环处理）
死区分为两种情况：
①在原地转了四次
即陷入四面都是黑色的区域，这样会导致一直在原地旋转，必须加以判定打破循环
②围绕一个点转了四次
一直处于这样的死循环，虽不会卡死程序（有brea_flag兜底），但这样的循环毫无意义，须加以判定打破循环。
六、自适应八向迷宫源代码
将上述方法都理解透彻后，相必读这段函数就不会那么费力了。这里将代码再贴一遍，一定注意形参的输入顺序。对着代码硬读可能很费力，不如画图来的直接。当时读bilibili博主“苏格拉没有底”的八邻域配合自适应阈值处理灰度图的代码时，前前后后读了几十遍，所以不要心急，一遍一遍读终会理解。另外特别注意 L_Data_Statics 和 R_Data_Statics 加一的时机。
//**************方向计算****************
//记录方向时，要将每个坐标变为单一数字，便于简化后续算法。例如八邻域的方向（逆时针 顺时针）
//3 2 1 1 2 3
//4 0 0 4
//5 6 7 7 6 5
/
//{-1,-1},{0,-1},{+1,-1},
//{-1, 0}, {+1, 0},
//{-1,+1},{0,+1},{+1,+1},
//迷宫缺点是无法像八邻域一样逐步记录方向，但八邻域无非就是以上八个点，且与中心点差值固定，不会随迷宫算法的朝向和移动方向而改变，因此先将每个横坐标乘 3
//{-3,-1},{0,-1},{+3,-1},
//{-3, 0}, {+3, 0},
//{-3,+1},{0,+1},{+3,+1},
//再将横坐标减去纵坐标
// -2, 1, 4
// -3, 3
// -4,-1, 2
//由此可以得到一个八邻方向坐标。只需在每次移动后，在方向数组里记录对应数字，就可确定生长方向
//此算法无任何原理，只是为了得到八个不一样的值可以用来判定方向，横坐标可以乘大于 2 的任意值，2以内会出现重复
//************************************
//******************自适应方向迷宫参数************************
const int8 L_Face_Dir[4][2] = {{0,-1},{1,0},{0,1},{-1,0}}; //左侧迷宫面向
// 0
//3 1
// 2
const int8 L_Face_Dir_L[4][2] = {{-1,-1},{1,-1},{1,1},{-1,1}}; //左侧面向的左前方
//0 1
//
//3 2
const int8 R_Face_Dir[4][2] = {{0,-1},{1,0},{0,1},{-1,0}}; //右侧迷宫面向
// 0
//3 1
// 2
const int8 R_Face_Dir_R[4][2] = {{1,-1},{1,1},{-1,1},{-1,-1}}; //右侧面向的右前方
//3 0
//
//2 1
const int8 Square_0[25][2] = { //一个5 * 5的矩阵，用来求中心点周围的局部阈值（局部阈值）
{-2,-2},{-1,-2},{0,-2},{+1,-2},{+2,-2},
{-2,-1},{-1,-1},{0,-1},{+1,-1},{+2,-1},
{-2,-0},{-1, 0},{0, 0},{+1, 0},{+2,-0},
{-2,+1},{-1,+1},{0,+1},{+1,+1},{+2,+1},
{-2,+2},{-1,+2},{0,+2},{+1,+2},{+2,+2}
};
//迷宫单侧停止爬线标志位
uint8 L_Stop_Flag = 0;
uint8 R_Stop_Flag = 0;
//**********************************************************
/******
* 函数功能： 求取赛道二维数组边线
* 特殊说明： 基于上交代码的自适应迷宫优化后的自适应八向迷宫
* 形 参： uint16 Break_Flag 最大循环次数，防止卡死程序，一般为3~4倍图像宽度
* uint8(*image)[Image_X] 提取边线的图像
* uint8(*l_line)[2] 存放左侧边线的二维数组
* uint8(*r_line)[2] 存放右侧边线的二维数组
* int8 *l_dir 存放左侧边线每个点的生长方向
* int8 *r_dir 存放右侧边线每个点的生长方向
* uint16 *l_stastic 记录左侧边线点的个数
* uint16 *r_stastic 记录右侧边线点的个数
* uint8 *x_meet 记录左右两侧爬线相遇点的X坐标
* uint8 *y_meet 记录左右两侧爬线相遇点的Y坐标
* uint8 l_start_x 左侧爬线起始点的X坐标
* uint8 l_start_y 左侧爬线起始点的Y坐标
* uint8 r_start_x 右侧爬线起始点的X坐标
* uint8 r_start_y 右侧爬线起始点的Y坐标
* uint8 clip_value 计算每个阈值时相加的经验值，一般为-5 ~ 5，避免强行分割，可直接设为0
*
* 示例： Dir_Labyrinth_5((uint16)Use_Num, Find_Line_Image, Adaptive_L_Line, Adaptive_R_Line, Adaptive_L_Grow_Dir, Adaptive_R_Grow_Dir, &Adaptive_L_Statics, &Adaptive_R_Statics, &Adaptive_X_Meet, &Adaptive_Y_Meet,
Adaptive_L_Start_Point[0], Adaptive_L_Start_Point[1], Adaptive_R_Start_Point[0], Adaptive_R_Start_Point[1], 0);
* 返回值： 无
*/
void Dir_Labyrinth_5(uint16 Break_Flag, uint8(*image)[Image_X], uint8(*l_line)[2], uint8(*r_line)[2], int8 *l_dir, int8 *r_dir, uint16 *l_stastic, uint16 *r_stastic, uint8 *x_meet, uint8 *y_meet,
uint8 l_start_x, uint8 l_start_y, uint8 r_start_x, uint8 r_start_y, uint8 clip_value)
{
uint8 j = 0;
L_Stop_Flag = 0;
R_Stop_Flag = 0;
//左边变量
uint8 L_Center_Point[2] = {0}; //存放每次找到的XY坐标
uint16 L_Data_Statics = 0; //统计左边找到的边线点的个数
uint8 L_Front_Value = 0; //左侧 面向的前方点的灰度值
uint8 L_Front_L_Value = 0; //左侧 面向的左前方点的灰度值
uint8 L_Dir = 0; //此参数用于转向
uint8 L_Turn_Num = 0; //记录转向次数，若中心点前后左右都是黑色像素，就会在一个点转向四次，记录到四次时退出循环防止卡死程序
uint16 L_Pixel_Value_Sum = 0; //中心点与周围24个点的像素值和
float L_Thres = 0; //局部阈值,即L_Pixel_Value_Sum / 25
//右边变量
uint8 R_Center_Point[2] = {0}; //存放每次找到的XY坐标
uint16 R_Data_Statics = 0; //统计右边找到的边线点的个数
uint8 R_Front_Value = 0; //右侧 面向的前方点的灰度值
uint8 R_Front_R_Value = 0; //右侧 面向的左前方点的灰度值
uint8 R_Dir = 0; //此参数用于转向
uint8 R_Turn_Num = 0; //记录转向次数，若中心点前后左右都是黑色像素，就会在一个点转向四次，记录到四次时退出循环防止卡死程序
uint16 R_Pixel_Value_Sum = 0; //中心点与周围24个点的像素值和
float R_Thres = 0; //局部阈值
//第一次更新坐标点 将找到的起点值传进来
L_Center_Point[0] = l_start_x + 1;//x
L_Center_Point[1] = l_start_y;//y
R_Center_Point[0] = r_start_x - 1;//x
R_Center_Point[1] = r_start_y;//y
//开启方向迷宫循环
while (Break_Flag--)
{
//左边
//判定出死区后，挂出停止标志位，单侧爬线停止。
if(L_Stop_Flag == 0)
{
l_line[L_Data_Statics][0] = L_Center_Point[0]; //找到的中心点X坐标计入左边线数组
l_line[L_Data_Statics][1] = L_Center_Point[1]; //找到的中心点Y坐标计入左边线数组
if(L_Data_Statics != 0)
{
switch(l_dir[L_Data_Statics - 1]) //下面这一坨可以根据上一个点的生长方向大幅优化爬线时间
{
//从第二个点开始，第一个点的阈值要25个点全部加一遍
//当横向或纵向生长时，将原先的25次加法运算简化为十次计算
//当斜向生长时，将原先的25次加法计算简化为十八次运算
//可以画出图来，跟着代码走几遍，就可以很快速的理解
case 1:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 3][L_Center_Point[0] + 2] - image[L_Center_Point[1] + 3][L_Center_Point[0] + 1]
- image[L_Center_Point[1] + 3][L_Center_Point[0] + 0] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 1]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] - 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2];
break;
}
case -2:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 1][L_Center_Point[0] + 3] - image[L_Center_Point[1] - 0][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 1][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 2][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 3][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 3][L_Center_Point[0] + 2]
- image[L_Center_Point[1] + 3][L_Center_Point[0] + 1] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 0]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 0][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 0] + image[L_Center_Point[1] - 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2];
break;
}
case -3:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 2][L_Center_Point[0] + 3] - image[L_Center_Point[1] - 1][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 0][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 1][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 2][L_Center_Point[0] + 3]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] - 0][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2];
break;
}
case -4:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 3][L_Center_Point[0] - 1] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 0]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 1] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 2]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 3] - image[L_Center_Point[1] - 2][L_Center_Point[0] + 3]
- image[L_Center_Point[1] - 1][L_Center_Point[0] + 3] - image[L_Center_Point[1] + 0][L_Center_Point[0] + 3]
- image[L_Center_Point[1] + 1][L_Center_Point[0] + 3]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 0][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] - 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 0] + image[L_Center_Point[1] + 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2];
break;
}
case -1:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] - 3][L_Center_Point[0] - 2] - image[L_Center_Point[1] - 3][L_Center_Point[0] - 1]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 0] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 1]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2] + image[L_Center_Point[1] + 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] + 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2];
break;
}
case 2:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 1][L_Center_Point[0] - 3] - image[L_Center_Point[1] + 0][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 1][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 2][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 3][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 3][L_Center_Point[0] - 2]
- image[L_Center_Point[1] - 3][L_Center_Point[0] - 1] - image[L_Center_Point[1] - 3][L_Center_Point[0] + 0]
- image[L_Center_Point[1] - 3][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 0][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] + 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] - 2];
break;
}
case 3:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 2][L_Center_Point[0] - 3] - image[L_Center_Point[1] + 1][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 0][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 1][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 2][L_Center_Point[0] - 3]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] + 0][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2];
break;
}
case 4:
{
L_Pixel_Value_Sum = L_Pixel_Value_Sum - image[L_Center_Point[1] + 3][L_Center_Point[0] + 1] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 0]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 1] - image[L_Center_Point[1] + 3][L_Center_Point[0] - 2]
- image[L_Center_Point[1] + 3][L_Center_Point[0] - 3] - image[L_Center_Point[1] + 2][L_Center_Point[0] - 3]
- image[L_Center_Point[1] + 1][L_Center_Point[0] - 3] - image[L_Center_Point[1] - 0][L_Center_Point[0] - 3]
- image[L_Center_Point[1] - 1][L_Center_Point[0] - 3]
+ image[L_Center_Point[1] + 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] + 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 0][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 1][L_Center_Point[0] + 2]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 2] + image[L_Center_Point[1] - 2][L_Center_Point[0] + 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] + 0] + image[L_Center_Point[1] - 2][L_Center_Point[0] - 1]
+ image[L_Center_Point[1] - 2][L_Center_Point[0] - 2];
break;
}
}
}
else
{
for (j = 0; j < 25; j++) //第一个阈值将25个点全部加一遍，后续点的阈值根据生长方向计算
{
L_Pixel_Value_Sum += image[L_Center_Point[1] + Square_0[j][1]][L_Center_Point[0] + Square_0[j][0]];
}
}
L_Thres = (L_Pixel_Value_Sum + Thres_Interfere) / Thres_Num_Interfere; //阈值为25个点灰度值的平均值
L_Thres -= clip_value; //将得到的灰度阈值减去一个经验值，用来优化判定
//这里为反向平滑滤波（不知网上是否有这种算法，此处为本人原创），用于适应由较暗到（局部）高亮区域，或反之的光线情况。后续详细讲解原理
if(Thres_Filiter_Flag_1 == 1 || Thres_Filiter_Flag_2 == 1)
{
if(L_Data_Statics > 3)
{
L_Thres = L_Thres * 1.3f - L_Thres_Record[L_Data_Statics - 1] * 0.2f - L_Thres_Record[L_Data_Statics - 2] * 0.1f;
}
}
L_Thres_Record[L_Data_Statics] = L_Thres;
L_Data_Statics++; //每找到一个点统计个数+1
L_Judge_Again: //L_Judge_Again 与 goto 配合使用
if(L_Stop_Flag == 0)
{
L_Front_Value = image[L_Center_Point[1] + L_Face_Dir[L_Dir][1]][L_Center_Point[0] + L_Face_Dir[L_Dir][0]]; //记录面向的前方点的灰度值
L_Front_L_Value = image[L_Center_Point[1] + L_Face_Dir_L[L_Dir][1]][L_Center_Point[0] + L_Face_Dir_L[L_Dir][0]]; //记录面向的左前方点的灰度值
if((float)L_Front_Value < L_Thres) //面向的前方点是黑色
{
L_Dir = (L_Dir + 1) % 4; //需右转一次
L_Turn_Num ++;
if(L_Turn_Num == 4) //死区处理
{
L_Stop_Flag = 1; //当前后左右都是黑色时，进入死区，停止左侧爬线
}
goto L_Judge_Again;
}
else if((float)L_Front_L_Value < L_Thres) //左前方点是黑色，前方点是白色
{
L_Center_Point[0] += L_Face_Dir[L_Dir][0];
L_Center_Point[1] += L_Face_Dir[L_Dir][1]; //向前走一步
l_dir[L_Data_Statics - 1] = (L_Face_Dir[L_Dir][0] * 3) - L_Face_Dir[L_Dir][1];
L_Turn_Num = 0;
}
else //左前方和前方都是白色点
{
L_Center_Point[0] += L_Face_Dir_L[L_Dir][0];
L_Center_Point[1] += L_Face_Dir_L[L_Dir][1]; //向左前方走一步
l_dir[L_Data_Statics - 1] = (L_Face_Dir_L[L_Dir][0] * 3) - L_Face_Dir_L[L_Dir][1];
L_Dir = (L_Dir + 3) % 4; //左转一次
L_Turn_Num = 0;
}
if(L_Data_Statics >= 4) //O环处理，即转了一圈后回到原处，也是一种死区，当立即停止爬线
{
if(l_line[L_Data_Statics][0] == l_line[L_Data_Statics - 4][0]&&
l_line[L_Data_Statics][1] == l_line[L_Data_Statics - 4][1])
{
L_Stop_Flag = 1;
}
}
}
}
//右侧与左侧同理，代码也类似，理解左侧后右侧就很简单
if(R_Stop_Flag == 0)
{
r_line[R_Data_Statics][0] = R_Center_Point[0];
r_line[R_Data_Statics][1] = R_Center_Point[1];
if(R_Data_Statics != 0)
{
switch(r_dir[R_Data_Statics - 1])
{
case 1:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 3][R_Center_Point[0] + 2] - image[R_Center_Point[1] + 3][R_Center_Point[0] + 1]
- image[R_Center_Point[1] + 3][R_Center_Point[0] + 0] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 1]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] - 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2];
break;
}
case -2:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 1][R_Center_Point[0] + 3] - image[R_Center_Point[1] - 0][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 1][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 2][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 3][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 3][R_Center_Point[0] + 2]
- image[R_Center_Point[1] + 3][R_Center_Point[0] + 1] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 0]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 0][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 0] + image[R_Center_Point[1] - 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2];
break;
}
case -3:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 2][R_Center_Point[0] + 3] - image[R_Center_Point[1] - 1][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 0][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 1][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 2][R_Center_Point[0] + 3]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] - 0][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2];
break;
}
case -4:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 3][R_Center_Point[0] - 1] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 0]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 1] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 2]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 3] - image[R_Center_Point[1] - 2][R_Center_Point[0] + 3]
- image[R_Center_Point[1] - 1][R_Center_Point[0] + 3] - image[R_Center_Point[1] + 0][R_Center_Point[0] + 3]
- image[R_Center_Point[1] + 1][R_Center_Point[0] + 3]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 0][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] - 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 0] + image[R_Center_Point[1] + 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2];
break;
}
case -1:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] - 3][R_Center_Point[0] - 2] - image[R_Center_Point[1] - 3][R_Center_Point[0] - 1]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 0] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 1]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2] + image[R_Center_Point[1] + 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] + 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2];
break;
}
case 2:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 1][R_Center_Point[0] - 3] - image[R_Center_Point[1] + 0][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 1][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 2][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 3][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 3][R_Center_Point[0] - 2]
- image[R_Center_Point[1] - 3][R_Center_Point[0] - 1] - image[R_Center_Point[1] - 3][R_Center_Point[0] + 0]
- image[R_Center_Point[1] - 3][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 0][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] + 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] - 2];
break;
}
case 3:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 2][R_Center_Point[0] - 3] - image[R_Center_Point[1] + 1][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 0][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 1][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 2][R_Center_Point[0] - 3]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] + 0][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2];
break;
}
case 4:
{
R_Pixel_Value_Sum = R_Pixel_Value_Sum - image[R_Center_Point[1] + 3][R_Center_Point[0] + 1] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 0]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 1] - image[R_Center_Point[1] + 3][R_Center_Point[0] - 2]
- image[R_Center_Point[1] + 3][R_Center_Point[0] - 3] - image[R_Center_Point[1] + 2][R_Center_Point[0] - 3]
- image[R_Center_Point[1] + 1][R_Center_Point[0] - 3] - image[R_Center_Point[1] - 0][R_Center_Point[0] - 3]
- image[R_Center_Point[1] - 1][R_Center_Point[0] - 3]
+ image[R_Center_Point[1] + 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] + 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 0][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 1][R_Center_Point[0] + 2]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 2] + image[R_Center_Point[1] - 2][R_Center_Point[0] + 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] + 0] + image[R_Center_Point[1] - 2][R_Center_Point[0] - 1]
+ image[R_Center_Point[1] - 2][R_Center_Point[0] - 2];
break;
}
}
}
else
{
for (j = 0; j < 25; j++)
{
R_Pixel_Value_Sum += image[R_Center_Point[1] + Square_0[j][1]][R_Center_Point[0] + Square_0[j][0]];
}
}
R_Thres = (R_Pixel_Value_Sum + Thres_Interfere) / Thres_Num_Interfere;
R_Thres -= clip_value;
if(Thres_Filiter_Flag_1 == 1 || Thres_Filiter_Flag_2 == 1)
{
if(R_Data_Statics > 3)
{
R_Thres = R_Thres * 1.3f - R_Thres_Record[R_Data_Statics - 1] * 0.2f - R_Thres_Record[R_Data_Statics - 2] * 0.1f;
}
}
R_Thres_Record[R_Data_Statics] = R_Thres;
R_Data_Statics++;
R_Judgme_Again:
if(R_Stop_Flag == 0)
{
R_Front_Value = image[R_Center_Point[1] + R_Face_Dir[R_Dir][1]][R_Center_Point[0] + R_Face_Dir[R_Dir][0]];
R_Front_R_Value = image[R_Center_Point[1] + R_Face_Dir_R[R_Dir][1]][R_Center_Point[0] + R_Face_Dir_R[R_Dir][0]];
if((float)R_Front_Value < R_Thres)
{
R_Dir = (R_Dir + 3) % 4;
R_Turn_Num ++;
if(R_Turn_Num == 4)
{
R_Stop_Flag = 1;
}
goto R_Judgme_Again;
}
else if((float)R_Front_R_Value < R_Thres)
{
R_Center_Point[0] += R_Face_Dir[R_Dir][0];
R_Center_Point[1] += R_Face_Dir[R_Dir][1];
r_dir[R_Data_Statics - 1] = R_Face_Dir[R_Dir][0] * 3 - R_Face_Dir[R_Dir][1];
R_Turn_Num = 0;
}
else
{
R_Center_Point[0] += R_Face_Dir_R[R_Dir][0];
R_Center_Point[1] += R_Face_Dir_R[R_Dir][1];
r_dir[R_Data_Statics - 1] = R_Face_Dir_R[R_Dir][0] * 3 - R_Face_Dir_R[R_Dir][1];
R_Dir = (R_Dir + 1) % 4;
R_Turn_Num = 0;
}
if(R_Data_Statics >= 4)
{
if(r_line[R_Data_Statics][0] == r_line[R_Data_Statics - 4][0]&&
r_line[R_Data_Statics][1] == r_line[R_Data_Statics - 4][1])
{
R_Stop_Flag = 1;
}
}
}
}
if(L_Stop_Flag == 0 && R_Stop_Flag == 0)
{
if ((My_ABS(r_line[R_Data_Statics - 1][0] - l_line[L_Data_Statics - 1][0]) <= 1)
&& (My_ABS(r_line[R_Data_Statics - 1][1] - l_line[L_Data_Statics - 1][1]) <= 1)) //两侧爬线相遇，退出循环，一张图像爬线结束
{
*y_meet = (r_line[R_Data_Statics - 1][1] + l_line[L_Data_Statics - 1][1]) >> 1; //记录相遇点Y
*x_meet = (r_line[R_Data_Statics - 1][0] + l_line[L_Data_Statics - 1][0]) >> 1; //记录相遇点X
break;
}
}
//有一侧存在死区时，对相遇点的判定放宽松一些，防止实际相遇但没有判定出，导致爬线紊乱的情况
else
{
if ((My_ABS(r_line[R_Data_Statics - 1][0] - l_line[L_Data_Statics - 1][0]) <= 3)
&& (My_ABS(r_line[R_Data_Statics - 1][1] - l_line[L_Data_Statics - 1][1]) <= 3)) //两侧爬线相遇，退出循环，一张图像爬线结束
{
*y_meet = (r_line[R_Data_Statics - 1][1] + l_line[L_Data_Statics - 1][1]) >> 1; //记录相遇点Y
*x_meet = (r_line[R_Data_Statics - 1][0] + l_line[L_Data_Statics - 1][0]) >> 1; //记录相遇点X
break;
}
}
}
L_Stop_Flag = 0;
R_Stop_Flag = 0;
*l_stastic = L_Data_Statics; //记录左侧边线点个数
*r_stastic = R_Data_Statics; //记录右侧边线点个数
}