种子填充赛道算法【智能车】

种子填充算法

一直感觉有用到种子的算法都很高级（像八领域扫线），没想到自己有一天也能用上。想到用种子算法是在队友用了连通域找车的时候，看到图片上一块一块的，就有了能不能把赛道单独给显示出来（赛道外的路障、其他杂物全部变成黑色的）的想法；于是去网上查填充算法，一般是两种，一种是四连通、八连通使用递归或栈找点填充，另一种是一行一行的查，找最后边的边界点作为种子，每次弹出一个种子向左右两边填充并寻找新种子的扫描线填充法。
一开始，我写的是八连通栈填充，很可惜开始没想到，每个像素遍历一圈后才把最后一个符合条件的像素压入栈（换句话说只能往一个方向填充。。。）后来一直没发现这个问题，于是改写扫描线算法（在灰度图上）

            uint16_t number_seed;
            uint8_t error_chazhi_1 = 5;
            uint8_t error_chazhi_2 = 5;
            uint8_t error_chazhi_3 = 6;//没找到种子且一定有要补的点时，寻找随机一个要补的点，差值可以小一些
            uint8_t seed_x = 93, seed_y = 0, chongfubianli1_flag = 0;//初始化一个种子
            uint8_t true_jiyuan;
            uint8_t must_findseed_flag = 0;
            true_jiyuan = P_Pixel[0][seed_x];
            for (j = 0; j < 185; j++)//寻找参考点
            {
                if (true_jiyuan - P_Pixel[0][j] < -20)
                {
                    seed_x = (uint8_t)(j + 0);
                }
            }
            true_jiyuan = P_Pixel[0][seed_x];
            true_pull_stack(seed_x, seed_y);
            SetText("找到的参考点:" + true_jiyuan);
            while (stack_top != 0)//条件是栈不空
            {
                center_seed = true_push_stack();
                number_seed = 0;
                chongfubianli1_flag = 0;
                R_black[0] = 0;
                L_black[0] = 185;
                //填充栈顶种子所在行
                //这里就是向左右两边填充，代码太多省掉了
                //最后向上下行分别寻找新种子，代码有点长而且有bug就不贴出来了
            }

https://www.cnblogs.com/JDomain/p/6555808.html
这里是扫描种子填充算法的思路，有很好的图解很方便理解算法思想。
最终的效果

没办法，由于实验室光线太差，边界条件实在太难写（边界灰度值变化规律很模糊，条件一直在变），很多时候算法效果很差，而且运算时间很长，放弃了。

之后又看了一遍八连通扫线法，忽然发现自己以前写的填充算法一直都忘了遍历一个点就可以判断一次是否入栈，于是修改了以前的代码，终于有点头绪了

stack_top = 0;
   connects[0].x0 = -1;
   connects[0].y0 = -1;
   connects[1].x0 = 0;
   connects[1].y0 = -1;
   connects[2].x0 = 1;
   connects[2].y0 = -1;
   connects[3].x0 = 1;
   connects[3].y0 = 0;
   connects[4].x0 = 1;
   connects[4].y0 = 1;
   connects[5].x0 = 0;
   connects[5].y0 = 1;
   connects[6].x0 = -1;
   connects[6].y0 = 1;
   connects[7].x0 = -1;
   connects[7].y0 = 0;

   size_point center_seed;
   uint8_t seed_x = 93, seed_y = 0;
   for (y = 0; y < 70; y++)
   {
       for (x = 0; x < 186; x++)
       {
           pixels_seed_black[y, x] = 0;
       }
   }

   maxvar = 0;//加入大津算法更好找到符合条件的像素点，也就是更好填充
   w0 = 0;
   u = 0;
   gray_hh = 0;
   var = 0;
   Thresholds[3] = 0;
   eta = 0;
   pg = 0;
   for (i = 0; i < 256; i++)
   { bin[i] = 0; }
   for (i = 0; i <= 69; i += 2)
   {
       for (j = 0; j <= 185; j += 2)
       {
           ++bin[P_Pixel[i][j]];
       }
   }
   size = 35 * 93;
   for (i = 0; i < 256; i++)
   {
       if (bin[i] == 0)
           continue;
       bin[i] = bin[i] / size;
       u += i * bin[i];
   }
   //创建比例灰度直方图
   for (i = 0; i < 80; i++)
   {
       if (bin[i] == 0)
           continue;
       w0 += bin[i];
       gray_hh += i * bin[i];             //灰度和
       u0 = gray_hh / w0;
       differ = (byte)My_Abs_float(u0 - (u - gray_hh) / (u - w0));
       var = (u0 - u) * (u0 - u) * w0 / (1 - w0);
       pg += (i - u) * (i - u) * bin[i];
       if (var > maxvar)
       {
           maxgray = gray_hh;
           maxbin = w0;
           maxvar = var;
           Thresholds[3] = (byte)i;
       }
   }
if (Thresholds[3] -  P_Pixel[0][93] < 5)//初始化三个种子中间和两个底角，防止弯道无法填充
		pull_stack(seed_x, seed_y);
if (Thresholds[3] - P_Pixel[0][0] < 5)
		pull_stack(0, 0);
if (Thresholds[3] - P_Pixel[0][185] < 5)
		pull_stack(185, 0);
while (stack_top != 0)
{
    center_seed = push_stack();
    for (i = 0; i < 8; i++)//向八个方向寻找新种子
    {
        int px = center_seed.x0 + connects[i].x0;
        int py = center_seed.y0 + connects[i].y0;
        if (px < 0 || py < 0 || px >= 186 || py >= 70)
            continue;
        if (pixels_seed_black[py, px] == black && Thresholds[3] - P_Pixel[py][px] < 5 && My_Abs(P_Pixel[py][px]- P_Pixel[center_seed.y0][center_seed.x0]) < 8)
        {
            pull_stack((uint8_t)px, (uint8_t)py);
        }
    }
}

然而时序还是炸裂了。开始优化代码。首先大津虽然好用但是固定一个阈值也很香呀，所以先删掉大津。再，赛道是连续的，不存在在类似下面这种情况，于是八连通砍成四连通。

这时候代码仍是需要18ms左右的时间（一副186*70的灰度图）。还要继续优化。
之后，一度陷入困境，想不到什么方法可以继续节省时序，然后我看了一下之前的扫描线，忽然想到，是否可以先把第一行的种子找出来，然后就不用向下寻找啦？试了试，发现大部分情况下都不需要向下寻找就可以填充完整个赛道（只有环岛出来的时候或者环岛的一小部分无法被填充）

while (stack_top != 0)
{
    center_seed = push_stack();
    for (i = 0; i < 3; i++)
    {
        int px = center_seed.x0 + connects[i].x0;
        int py = center_seed.y0 + connects[i].y0;
        if (px < 0 || py < 0 || px >= 186 || py >= 70)
            continue;
        if (pixels_seed_black[py, px] == black && Thresholds[3] - P_Pixel[py][px] < 5 && My_Abs(P_Pixel[py][px]- P_Pixel[center_seed.y0][center_seed.x0]) < 8)
        {
            pull_stack((uint8_t)px, (uint8_t)py);
        }
    }
}

优化到10ms了，最终，我把for展开，降到6.3ms，才终于可以用了（最终版了，优化了好几天，我太笨了。。）

struct size_point
{
	int x0;
	int y0;
};

struct size_point stack_seed[6000];//栈
uint16_t stack_top = 0;
uint8_t (*p_Pixels)[186] = &Pixels[0];

void pull_stack(uint8_t x, uint8_t y)//入栈
{
	*(*(p_Pixels + y) + x) = 1;
	stack_seed[stack_top].x0 = x;
	stack_seed[stack_top].y0 = y;
	stack_top++;
}
struct size_point push_stack()//出栈
{		
	stack_seed[stack_top].y0 = 0;
	stack_seed[stack_top].x0 = 0;
	return stack_seed[--stack_top];
}

int panbianjie(uint8_t x, uint8_t y)
{
	if (x + y == 0)
	{
			return 0;
	}
	return (int)((abs(x - y) * 100 / (x + y)) + 0.5f);
}

struct size_point connects[8]={	//八领域扫点
//{-1,-1},
//{0,-1},
//{1,-1},
{1,0},
//{1,1},
{0,1},
//{-1,1},
{-1,0}
};

void SignalProcess_grayfine_fill(void)
 {
		int j,px,py;
		struct size_point center_seed;
		uint8_t (*p_image)[188] = &image[30];
		p_Pixels = &Pixels[0];
		stack_top = 0;
		for (i = 0; i <= 69; i++)
		{
			for (j = 0; j <= 185; j++)
			{
					*(*(p_Pixels + i) + j) = 0;
			}
		}
		Ostu_Threshold = 50;
		
		for (i = 0; i < 186; i++)
		{
			if (Ostu_Threshold - *(*(p_image + 0) + i + 1) < 5)
					pull_stack((uint8)i, 0);
		}
		while (stack_top != 0)
		{
			center_seed = push_stack();
			px = center_seed.x0 + connects[0].x0;
			py = center_seed.y0 + connects[0].y0;
			if (*(*(p_Pixels + py) + px) == 1 || px < 0 || py < 0 || px >= 186 || py >= 70)
			{}
			else
			{
					if (abs(*(*(p_image + py) + px + 1)- *(*(p_image + center_seed.y0) + center_seed.x0 + 1)) < 8 && Ostu_Threshold - *(*(p_image + py) + px + 1) < 5)
					{
									pull_stack((uint8_t)px, (uint8_t)py);
					}
			}
			
			px = center_seed.x0 + connects[1].x0;
			py = center_seed.y0 + connects[1].y0;
			if (*(*(p_Pixels + py) + px) == 1 || px < 0 || py < 0 || px >= 186 || py >= 70)
			{}
			else
			{
					if (abs(*(*(p_image + py) + px + 1)- *(*(p_image + center_seed.y0) + center_seed.x0 + 1)) < 8 && Ostu_Threshold - *(*(p_image + py) + px + 1) < 5)
					{
							pull_stack((uint8_t)px, (uint8_t)py);
					}
			}
			
			px = center_seed.x0 + connects[2].x0;
			py = center_seed.y0 + connects[2].y0;
			if (*(*(p_Pixels + py) + px) == 1 || px < 0 || py < 0 || px >= 186 || py >= 70)
			{}
			else
			{
					if (abs(*(*(p_image + py) + px + 1)- *(*(p_image + center_seed.y0) + center_seed.x0 + 1)) < 8 && Ostu_Threshold - *(*(p_image + py) + px + 1) < 5)
					{
							pull_stack((uint8_t)px, (uint8_t)py);
					}
			}
		}
}

（以上运算都是在k66上测量滴）
不过每个车都要调一下参数，比如Ostu_Threshold 这个固定阈值，要根据自己车的阈值来调整，还有abs(((p_image + py) + px + 1)- ((p_image + center_seed.y0) + center_seed.x0 + 1)) < 8；这个是两个像素之间不能超过太多，也要根据自己的图像调整；Ostu_Threshold - ((p_image + py) + px + 1) < 5这个是不能低于固定阈值的多少，防止找到赛道外面


（甚至有点抑制边缘糊掉的功能。）
大津：2.3ms 遍历一次186*70的图像：1ms