智能车摄像头开源—1.1核心代码：自适应八向迷宫（上）
目录
一、开源项目简介
二、前期探索与算法确定
1.常用二值化算法对比（个人经验）
大津法
卷积核
差比和
自适应迷宫（八领域）
三、自适应八向迷宫
一、开源项目简介
2024.8.19，决战前夕，正式决定将十个多月来的心血进行开源。本在区赛打完之后，就计划如果国一，就部分开源，若是国二，就全部开源。但显然，在封车的那一刻，结局就已经注定，已经无望国一。所以此行怀揣失意而来，注定满载遗憾而去，但不甘心于自己所有的成果随着战败的我而隐去，因此在此将代码全部开源。
计划将整个摄像头代码进行详细讲述，并在后续陆续更新，做成一套完整的系列，供新入门智能车图像处理的学弟学妹提供参考。同时也郑重声明，此系列成果与经验均由本人实践得出，并不保证完全正确，仅供入门学习与参考。
本人将智能车摄像头图像处理大致分为四部分：图像基础处理与边线提取（底层），信息提取（中层），元素判断与元素处理（上层），以及速度决策与控制相关代码（链接层）。
本在考虑到底从何处讲起，但显然，图像的预处理与边线的提取为整个摄像头图像处理的基石，优秀的边线提取算法能够快速、准确且高效的完成任务，同时对强光以及明暗不均的情况有较好的适应性，不占用大量的运算资源。因此，第一篇就直接开门见山，将最关键的先行给出，后续再对其他处理进行补充说明。
二、前期探索与算法确定
1.常用二值化算法对比（个人经验）
大津法
想必大多数摄像头位入门后接触的第一个图像算法便是大津法，大津法简单易用，同时网上流传的开源代码大多是大津法，因此可以快速的移植并直接使用。（若还没接触过大津法的铁柱或金发妹可以先行去了解原理与算法的实现，这里不过多讲解原理，毕竟有非常多的开源项目可供参考）。但使用大津二值化图像，耗费时间较长，同时使用单阈值处理整幅图像，使其对局部较亮或较暗的情况表现并不理想。
针对于对光线适应性不佳的问题，网上又给出了分块大津，模糊大津等优化算法，在一定程度上提高了对光线的适应性，也优化了计算时间，但仍无法达到我满意的效果，且计算时长对主控的占用较长，使一些运算性能较低的主控压力拉满。若不想在此处花费时间，大津法还是比较推荐的，可以直接移植现成的代码后便可直接使用，得到二值化后的图像，但显然，这并不是我心目中的理想算法。
这里给出常规大津和三分割大津法：（算法来源于学长传承）
/************************************************************************
* 函数名称：short GetOSTU (unsigned char tmImage[LCDH][LCDW])
* 功能说明：大津法求阈值大小
* 参数说明：tmImage ： 图像数据
* 函数返回：无
* 修改时间：2011年10月28日
* 备 注： GetOSTU(Image_Use);//大津法阈值
Ostu方法又名最大类间差方法，通过统计整个图像的直方图特性来实现全局阈值T的自动选取，其算法步骤为：
1) 先计算图像的直方图，即将图像所有的像素点按照0~255共256个bin，统计落在每个bin的像素点数量
2) 归一化直方图，也即将每个bin中像素点数量除以总的像素点
3) i表示分类的阈值，也即一个灰度级，从0开始迭代 1
4) 通过归一化的直方图，统计0~i 灰度级的像素(假设像素值在此范围的像素叫做前景像素) 所占整幅图像
的比例w0， 并统计前景像素的平均灰度u0；统计i~255灰度级的像素(假设像素值在此范围的像素叫做背
景像素) * 所占整幅图像的比例w1，并统计背景像素的平均灰度u1；
5) 计算前景像素和背景像素的方差 g = w0*w1*(u0-u1) (u0-u1)
6) i++；转到4)，直到i为256时结束迭代
7) 将最大g相应的i值作为图像的全局阈值
缺陷:OSTU算法在处理光照不均匀的图像的时候，效果会明显不好，因为利用的是全局像素信息。
************************************************************************/
short GetOSTU (unsigned char tmImage[LCDH][LCDW])
{
signed short i, j;
unsigned long Amount = 0;
unsigned long PixelBack = 0;
unsigned long PixelshortegralBack = 0;
unsigned long Pixelshortegral = 0;
signed long PixelshortegralFore = 0;
signed long PixelFore = 0;
float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差;
signed short MinValue, MaxValue;
signed short Threshold = 0;
unsigned char HistoGram[256] = {0}; //初始化灰度直方图
for (j = 0; j < LCDH; j++)
{
for (i = 0; i < LCDW; i++)
{
HistoGram[tmImage[j][i]]++; //统计灰度级中每个像素在整幅图像中的个数
}
}
for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++); //获取最小灰度的值
for (MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--); //获取最大灰度的值
if (MaxValue == MinValue)
return MaxValue; // 图像中只有一个颜色
if (MinValue + 1 == MaxValue)
return MinValue; // 图像中只有二个颜色
for (j = MinValue; j <= MaxValue; j++)
Amount += HistoGram[j]; // 像素总数
Pixelshortegral = 0;
for (j = MinValue; j <= MaxValue; j++)
{
Pixelshortegral += HistoGram[j] * j; //灰度值总数
}
SigmaB = -1;
for (j = MinValue; j < MaxValue; j++)
{
PixelBack = PixelBack + HistoGram[j]; //前景像素点数
PixelFore = Amount - PixelBack; //背景像素点数
OmegaBack = (float) PixelBack / Amount; //前景像素百分比
OmegaFore = (float) PixelFore / Amount; //背景像素百分比
PixelshortegralBack += HistoGram[j] * j; //前景灰度值
PixelshortegralFore = Pixelshortegral - PixelshortegralBack; //背景灰度值
MicroBack = (float) PixelshortegralBack / PixelBack; //前景灰度百分比
MicroFore = (float) PixelshortegralFore / PixelFore; //背景灰度百分比
Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore); //计算类间方差
if (Sigma > SigmaB) //遍历最大的类间方差g //找出最大类间方差以及对应的阈值
{
SigmaB = Sigma;
Threshold = j;
}
}
// Threshold += 40;
return Threshold; //返回最佳阈值;
}
/************************************************************************
* 函数名称：void Get_Bin_Image_seg (unsigned char mode)
* 功能说明：分割大津法,将图片水平分成三部分，分别通过大津法取阈值，判断每部分的阈值与全局阈值的差别，如果相差太大 ，使用全局阈值。
* 参数说明：
* 函数返回：无
* 修改时间：2022年7月28日
* 备 注： Get_Bin_Image(0); //使用大津法二值化
************************************************************************/
void Get_Bin_Image_seg()
{
uint8 Thresholds[3]={0}; //存储三部分阈值
uint8 threshold1=0;
uint8 (*p_image)[94] = &OLED_Bin_data[0];
// 计算全局阈值，在下面分成三个部分，分别计算各部分的阈值，如果和全局阈值偏差过大，则使用全局阈值
threshold1= SimplifyGetOSTU(OLED_Bin_data[0],LCDW,LCDH);
for(k=0; k<3; k++)
{
maxvar = 0;
w0 = 0;
u = 0;
gray_hh = 0;
u0tmp = 0;
var = 0;
Thresholds[k] = 0;
// 初始化灰度比例直方图
for (i = 0; i < 256; i++)
{
bin_float[i] = 0;
}
uint32 gray_sum=0;
for (i = nchu_point[k].y0; i <= nchu_point[k].y1; i++)
{
for (j = nchu_point[k].x0; j <= nchu_point[k].x1; j++)
{
++bin_float[*(*(p_image + i) + j)];
gray_sum+=*(*(p_image + i) + j);
}
}
size = (nchu_point[k].y1 - nchu_point[k].y0 + 1) * (nchu_point[k].x1 - nchu_point[k].x0 + 1);
for (i = 0; i < 256; i++)
{
bin_float[i] = bin_float[i] / size; // 每个灰度值的像素点所占比例
u += i * bin_float[i];
}
//创建比例灰度直方图
for (i = 0; i < 256; i++)
{
/**
w0 += bin_float[i];
gray_hh += i * bin_float[i]; //灰度和
u0 = gray_hh / w0;
var = (u0 - u) * (u0 - u) * w0 / (1 - w0);
if (var > maxvar)
{
maxgray = gray_hh;
maxbin = w0;
maxvar = var;
Thresholds[k] = (uint8)i;
}**/
w0 += bin_float[i];//背景部分每个灰度值的像素点所占比例之和
u0tmp += i * bin_float[i]; //灰度和
w1 = 1-w0;
u1tmp = gray_sum/size - u0tmp;
u0 = u0tmp / w0;
u1 = u1tmp / w1;
u = u0tmp+ u1tmp;
var = (u0 - u) * (u0 - u) * w0 / (1 - w0);
if (var > maxvar)
{
maxgray = u0tmp;
maxbin = w0;
maxvar = var;
Thresholds[k] = (uint8)i;
}
}
if (k == 0)
{
if (gray_hh > 15 && gray_hh <= 33)
{
if (maxbin < 0.9f)
{
Thresholds[k] = (uint8)(Thresholds[1] - 3);
}
}
else if (gray_hh > 41 && gray_hh <= 47)
{
if (maxbin < 0.64f || maxbin > 0.76f)
{
Thresholds[k] = (uint8)(Thresholds[1] - 3);
}
}
else if (gray_hh > 50 && gray_hh <= 60)
{
if (maxbin < 0.42f || maxbin > 0.58f)
{
Thresholds[k] = (uint8)(Thresholds[1] - 3);
}
}
if (fabs(threshold1 - Thresholds[k]) >= 30)//相差太大直接用标准值
{
Thresholds[k] = threshold1;
}
else
{
Thresholds[k] = (uint8)(Thresholds[k] + 0.5f * (threshold1 - Thresholds[k]));//这里是取了平均，可以通过调整占比适应不同的光照条件
}
}
else if (k == 1)
{
if(gray_hh > 69 && gray_hh < 80)
{
if (maxbin > 0.15f)
{
Thresholds[k] = (uint8)(Thresholds[0] + 3);
}
}
if (fabs(threshold1 - Thresholds[k]) >= 30)//相差太大直接用标准值
{
Thresholds[k] = threshold1;
}
else
{
Thresholds[k] = (uint8)(Thresholds[k] + 0.5f * (threshold1 - Thresholds[k]));//这里是取了平均，可以通过调整占比适应不同的光照条件
}
}
else if (k == 2)
{
if (maxbin < 0.85f && gray_hh < 28)
{
Thresholds[k] = (uint8)(Thresholds[1] - 3);
}
else if(gray_hh > 69 && gray_hh < 79)
{
if (maxbin < 0.5f || maxbin > 0.15f)
{
Thresholds[k] = (uint8)(Thresholds[1] - 3);
}
}
if (fabs(threshold1 - Thresholds[k]) >= 30)//相差太大直接用标准值
{
Thresholds[k] = threshold1;
}
else
{
Thresholds[k] = (uint8)(Thresholds[k] + 0.5f * (threshold1 - Thresholds[k]));//这里是取了平均，可以通过调整占比适应不同的光照条件
}
}
for (i = 0; i < Thresholds[k]; i++)
{
Bin_Array[i] = 0;
}
for (i = Thresholds[k]; i < 256; i++)
{
Bin_Array[i] = 255;
}
for (i = nchu_point[k].y0; i <= nchu_point[k].y1; i++)
{
for (j = nchu_point[k].x0; j <= nchu_point[k].x1; j++)
{
Pixle[i][j] = Bin_Array[*(*(p_image + i) + j)];
}
}
}
}
/************************************************************************
* 函数名称：void Get_Bin_Image (unsigned char mode)
* 功能说明：图像二值化到Bin_Image[][]
* 参数说明：mode ：
* 0：使用大津法阈值
* 1：使用平均阈值
* 2: sobel 算子改进型 手动阈值，同时输出改为提取边沿的图像
* 3：sobel 算子改进型 动态阈值，同时输出改为提取边沿的图像
* 函数返回：无
* 修改时间：2020年10月28日
* 备 注： Get_Bin_Image(0); //使用大津法二值化
************************************************************************/
void Get_Bin_Image (unsigned char mode)
{
unsigned short i = 0, j = 0;
unsigned short Threshold = 0;
unsigned long tv = 0;
if (mode == 0)
{
Threshold = GetOSTU(OLED_Bin_data); //大津法阈值
}
if (mode == 1)
{
//累加
for (i = 0; i < LCDH; i++)
{
for (j = 0; j < LCDW; j++)
{
tv += OLED_Bin_data[i][j]; //累加
}
}
Threshold =(unsigned short)(tv / LCDH / LCDW); //求平均值,光线越暗越小，全黑约35，对着屏幕约160，一般情况下大约100
Threshold = Threshold + 20; //此处阈值设置，根据环境的光线来设定
}
// 二值化 //
for (i = 0; i < LCDH-1; i++)
{
for (j = 0; j < LCDW-1; j++)
{
if (OLED_Bin_data[i][j] > Threshold) //数值越大，显示的内容越多，较浅的图像也能显示出来
Pixle[i][j] = 1;//白
else
Pixle[i][j] = 0;//黑
}
}
}卷积核
使用卷积核提取边缘进而二值化图像不失为一个理想的选择。常用的卷积核包括Sobel卷积核，拉普拉斯卷积核等，龙邱在其官方例程中给出了基于Sobel的二值化算法，代码如下：
void sobelAutoThreshold (u8 source[MT9V03X_DVP_H/2][MT9V03X_DVP_W],u8 target[MT9V03X_DVP_H/2][MT9V03X_DVP_W])
{
/** 卷积核大小 */
short KERNEL_SIZE = 3;
short xStart = KERNEL_SIZE / 2;
short xEnd = MT9V03X_DVP_W - KERNEL_SIZE / 2;
short yStart = KERNEL_SIZE / 2;
short yEnd = MT9V03X_DVP_H - KERNEL_SIZE / 2;
short i, j, k;
short temp[3];
for (i = yStart; i < yEnd; i++)
{
for (j = xStart; j < xEnd; j++)
{
/* 计算不同方向梯度幅值 */
temp[0] = -(short) source[i - 1][j - 1] + (short) source[i - 1][j + 1] // {-1, 0, 1},
- (short) source[i][j - 1] + (short) source[i][j + 1] // {-1, 0, 1},
- (short) source[i + 1][j - 1] + (short) source[i + 1][j + 1]; // {-1, 0, 1};
temp[1] = -(short) source[i - 1][j - 1] + (short) source[i + 1][j - 1] // {-1, -1, -1},
- (short) source[i - 1][j] + (short) source[i + 1][j] // { 0, 0, 0},
- (short) source[i - 1][j + 1] + (short) source[i + 1][j + 1]; // { 1, 1, 1};
temp[2] = -(short) source[i - 1][j] + (short) source[i][j - 1] // {0, -1, -1},
- (short) source[i][j + 1] + (short) source[i + 1][j] // {1, 0, -1},
- (short) source[i - 1][j + 1] + (short) source[i + 1][j - 1]; // {1, 1, 0};
temp[3] = -(short) source[i - 1][j] + (short) source[i][j + 1] // {-1, -1, 0},
- (short) source[i][j - 1] + (short) source[i + 1][j] // {-1, 0, 1},
- (short) source[i - 1][j - 1] + (short) source[i + 1][j + 1]; // {0, 1, 1};
temp[0] = abs(temp[0]);
temp[1] = abs(temp[1]);
temp[2] = abs(temp[2]);
temp[3] = abs(temp[3]);
/* 找出梯度幅值最大值 */
for (k = 1; k < 3; k++)
{
if (temp[0] < temp[k])
{
temp[0] = temp[k];
}
}
/* 使用像素点邻域内像素点之和的一定比例 作为阈值 */
temp[3] =
(short) source[i - 1][j - 1] + (short) source[i - 1][j] + (short) source[i - 1][j + 1]
+ (short) source[i][j - 1] + (short) source[i][j] + (short) source[i][j + 1]
+ (short) source[i + 1][j - 1] + (short) source[i + 1][j] + (short) source[i + 1][j + 1];
if (temp[0] > (temp[3] / 12.0f))
{
target[i][j] = 0xFF;
}
else
{
target[i][j] = 0x00;
}
}
}
}
————————————————
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
原文链接：https://blog.csdn.net/fgfgfdg8/article/details/126403306卷积核在图像处理中有着非常普遍的使用。其中Sobel主要用于边缘检测，其原理是使用卷积核（即一个3*3的数字矩阵）计算每一个中心点灰度值变化最大的梯度方向。例如一个3*3区域内，第一行三个点的灰度值分别为：200， 200， 200，而最下面一行的三个点灰度值分别为：50， 50， 50，那么可这样计算：ABS((a1*200 + a2*200 + a3*200）- (b1*50 + b2*50 + b3*50)）,得出的结果与一个阈值进行比较（a1，a2，a3，b1，b2，b3为系数）。在这种情况下，灰度值由上到下有极其剧烈的变化，那么这个地方一个较大的梯度方向便是由上往下，即可以判定存在边缘（举例子，实际上并不会出现这么好的数值）。同样的，卷积核一般计算四个方向，分别为左右、上下和两个对角的梯度变化。这里简单讲解原理，可以搜索卷积核的资料进行详细学习。
回到龙邱代码中，其函数输入第一参数为要处理的图像，第二参数为用于存储处理后的图像。改变最后一步 if 语句中temp[3]所除的系数，即可改变对边线提取的精细程度（即改变梯度对比的阈值，梯度值大于这个阈值即判定存在边界，越精细会导致存在的噪点更多，需要后续更多的降噪处理，不宜灵敏度过大，否则噪点会严重影响边线提取），而改变最后一步中 if 语句中的“ > ”为 “ < ”可将黑白反向，当读懂代码后，黑白反向原理就很简单了，不需过多赘述。
卷积核本身具备对光线较好的适应性，但显然运算时间也较长，与大津相差不大，相比于大津，若不想在边线提取深究，建议选用卷积核将图像二值化，个人认为在一定程度上效果要优于大津法。
其中也曾偶然得到优化后的Sobel算法，但并未进行验证，这里直接给出，效果自行检验：
//imag[cam_w][cam_h]原图像数组
//imag1[cam_w][cam_h]处理后图像数组
//
//180mhz 主频lpcdan片机处理60 * 120图像实测7.2ms
//如果需要更快的处理速度可选择局部处理提高速度
* /
//#define cam_h 60 //高度（行）
//#define cam_w 120 //宽度（列）
void sobel(uint8_t imag[cam_h][cam_w], uint8_t imag1[cam_h][cam_w])
{
int tempx = 0, tempy = 0, temp = 0, i = 0, j = 0;
for (i = 1; i < cam_h - 1; i++)
{
for (j = 1; j < cam_w - 1; j++)
{
tempx = (-imag[i - 1][j - 1]) + (-2 * imag[i][j - 1]) + (-imag[i + 1][j - 1])
+ (imag[i - 1][j + 1]) + (2 * imag[i][j + 1]) + (imag[i + 1][j + 1]);
if (tempx < 0)
{
tempx = -tempx;
}
tempy = (imag[i + 1][j - 1]) + (2 * imag[i + 1][j]) + (imag[i + 1][j + 1])
+ (-imag[i - 1][j - 1]) + (-2 * imag[i - 1][j]) + (-imag[i - 1][j + 1]);
if (tempy < 0)
{
tempy = -tempy;
}
temp = tempx + tempy;
if (temp > 255)
{
temp = 255;
}
imag1[i][j] = temp;
}
}
}
temp = sqrt(tempx * tempx + tempy * tempy);
//这里加点我的理解：
//1.可将上述函数内：temp = tempx + tempy 替换为 temp = sqrt(tempx * tempx + tempy * tempy) 以优化
//2.算法整体好像并没有对图像二值化，只是将梯度结果限幅后丢给图像。至于二值化可以参考上述龙邱的方法
//3.龙邱方法是比对四个方向内的梯度最大值后与阈值比较，但此处算法不同，需读者仔细斟酌，至于怎么处理就得依靠读者发挥了差比和
不久前逐飞官方的摄像头培训中给出了差比和求取边线的方法，原理为：从最长白直列开始，向左（向右）依次寻找边线点，对于左侧找边线，用当前点的灰度值减去右侧相距五个点的灰度值，再将差值除二者的和值并取绝对值，再将结果向左移7位（乘128倍，使用移位比直接乘更节省算力和时间，故不使用乘100），将最后结果与一个阈值进行比较，当大于阈值时，说明当前点灰度值变化剧烈，即可判定为赛道黑边界。右侧边线同理，只是对比的点位于当前点的左侧。
此算法对光线变化表现出较好的适应性能，同时在我们双核200Mhz的主控上，单核内处理十字一张188 * 120图像达到了0.6ms的运算速度，极其之快，显然是一个非常不错的理想算法，可以快速地提取出赛道的一维边线数组。代码这里不列出，可自行在Bilibili中搜索逐飞科技找到代码实现方法并进行后续优化。（注：逐飞并没有将算法进行适应性优化，且给出的对比阈值固定，任何好的算法最终都需要非常多的优化才能表现出较好的适应性和效果，想使用逐飞方案，显然需要自行对算法进行进一步处理）。至于我为什么没给出来，因为懒的写，而且原理很简单，如果真想学真想用，大可不必纠结没有完善的开源代码，最好的代码就在你手里。
有些开源方案中，对a和b差值取绝对值，但我们只需要由白赛道对黑线的对比，不需由黑到白的对比，显然不取绝对值可有效防止误判。
在本人算法中也有使用差比和的代码部分，这里给出源码：
uint8 Compare_Value = 20;
//差比和 ****
//第一形参a为灰度值较大的点，第二形参为灰度值较低的点
int16 Compare_Num(int16 a, int16 b, int8 compare_value) //****
{
if((((a - b) << 7) / (a + b)) > compare_value)
{
return 1;
}
else
{
return 0;
}
}自适应迷宫（八领域）
在算法的探索中，以上算法或是速度，或是对光线的适应能力，都不能兼具，（除差比和方案外，但显然，差比和提取的信息量较少，每行左右边线各只有一个点，即一维边线，不利于后续的处理）以至于我始终不能确定最终的算法。但在网上寻找思路的过程中，接触到了上交AuTo战队开源出的自适应迷宫算法，使我豁然开朗。自适应是选用局部5×5区域内的像素灰度值均值作为当前区域分割黑白的阈值，而迷宫是一种爬线算法，可用八领域替代（当然，迷宫不输八邻域），上交战队将边线点的求取与爬线算法相结合。一方面，小局部阈值对明暗不均的图像表现出极好的适应性，同时边爬线边求取边线点，避免了对赛道外以及赛道内无关图像的处理（只对边界进行提取），详细资料可以去网上找上交开源代码，仔细研读后，定会受益匪浅。
同时，b站上名为“__苏格拉没有底___”的开源博主也给出了自适应八领域直接处理灰度图求出边线的源代码，原理与上交方案相同，都是直接处理灰度图像，只是将爬线算法更换为八领域，但二者相差不大。也可自行搜索获取。
在理解透彻二者的方案后，我意识到这种直接处理灰度图的方式具备极高的上限，也惊叹于开源作者的能力与灵感。意识到这就是我一直渴求的边线提取算法，因此我毅然决然的选定这种算法作为我摄像头边线提取的核心算法。
三、自适应八向迷宫
在确定最终算法思路后，我又对代码进行更深的分析与理解，意识到仍然存在优化空间，因此将迷宫的速度优势与八邻域的方向优势相结合，给出自己的八向迷宫，对每一个点的阈值计算进行进一步优化，同时扩充爬线时获取的信息量，用于后续拐点判定和元素处理。经实测，在我们双核200MHz主控单核处理一张188 * 120的图像（包括压缩），达到了0.3ms左右的极速（其中还包含一系列处理，会在后续给出），同时，算法对于不同光线场景均表现出很好的适应性。对于本人来说，已是极为理想的摄像头算法，当然，还需后来者评判。
本人认为对于此算法，在运算速度方面的优化已经无限接近于极致，当然，可能仍然存在优化空间等待后来者的探索，本人能力有限，已无法给出更多的优化思路。
最快速的理解方式，便是对着代码的运行思路，画图一步一步理解，自适应八向迷宫对于新接触到此算法的车友有一些难度，但其实真正理解后会发现很简单，此算法的特点就是简单粗暴，以至于大道至简。
因篇幅原因，这里先将源代码给出，在下篇文章中再进行详细讲述：
//**************方向计算****************
//记录方向时，要将每个坐标变为单一数字，便于简化后续算法。例如八邻域的方向（逆时针 顺时针）
//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 >= 5) //O环处理，即转了一圈后回到原处，也是一种死区，当立即停止爬线
{
if(l_line[L_Data_Statics - 1][0] == l_line[L_Data_Statics - 5][0]&&
l_line[L_Data_Statics - 1][1] == l_line[L_Data_Statics - 5][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 >= 5)
{
if(r_line[R_Data_Statics - 1][0] == r_line[R_Data_Statics - 5][0]&&
r_line[R_Data_Statics - 1][1] == r_line[R_Data_Statics - 5][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; //记录右侧边线点个数
}最后还是建议不要一味地拷代码，一定要自己一点一点敲出来，真正把算法理解透彻才可谈优化算法。车赛不是拷代码大赛，更多的是学新东西、学新思路。
同时补充一点，核心算法的稳定性，决定了后续元素处理的方法；核心算法的速度，决定了整体代码的上限和部分控制上限；核心算法提取的信息量，决定了后续处理元素的难易程度。所以务必在此部分下功夫，确保理解透彻。
智能车摄像头开源—1.1 核心代码：自适应八向迷宫（上）
智能车摄像头开源—1.2 核心算法：自适应八向迷宫（下）
智能车摄像头开源—2 摄像头基础参数设置经验分享
智能车摄像头开源—3 图像基础处理、迭代优化与效果展示