int road;
/*
函数名称：int my_abs(int value)
功能说明：求绝对值
参数说明：
函数返回：绝对值
修改时间：
备 注：
example： my_abs( x)；
*/
//摄像头高110cm左右,以环岛为基准，图像20/30行后是赛道
//丁字尺测62.5到最底下挥着62到90
/*
* @brief 根据边线斜率来拟合中线
* @param 存放中线的数组
* @param 第一个点的坐标
* @param 第二个点的坐标
* @param 拟合中线的起始点坐标
* */
void Fitting_Middle_Line_With_Border(int* middle_line, int first_x, int second_x, int first_y, int second_y, int start_x, int start_y, int *last_x, int *last_y)
{
int dx = second_x - first_x;
int dy = second_y - first_y;
float K = (float)dx / (float)dy;
float b = start_x-K*start_y;
int Y;
*middle_line = start_x;
for (int i = 1; i <= (-dy); i++)
{
Y = (int)((start_y-i)*K+b);
*(middle_line+i) = Y;
}
*last_x = Y;
*last_y = start_y+dy;
}
/*
* @brief 画出曲线
* @param 输出图像地址
* @param 第一个点的横坐标
* @param 第一个点的纵坐标
* @param 第二个点的横坐标
* @param 第二个点的纵坐标
* @param 整幅图像的横坐标
* */
float center_x, center_y;
int distance;
void Draw_Curve(int *imag_out, int first_point_x, int first_point_y, int second_point_x, int second_point_y, int Imag_x)
{
// float center_x, center_y;
int dy = first_point_y - second_point_y;
int temp_x, supplementary_point, deviation;
// int distance;
//求出两点间的距离
if (first_point_x < second_point_x)
{
distance = My_Sqrt((first_point_y - second_point_y)*(first_point_y - second_point_y) + (second_point_x - first_point_x)*(second_point_x - first_point_x));
}
else
{
distance = My_Sqrt((first_point_y - second_point_y)*(first_point_y - second_point_y) + (first_point_x - second_point_x)*(first_point_x - second_point_x));
}
//求出圆心坐标
float c1 = (float)(second_point_x*second_point_x - first_point_x*first_point_x + second_point_y*second_point_y - first_point_y*first_point_y) / (2 *(second_point_x - first_point_x));
float c2 = (float)(second_point_y - first_point_y) / (second_point_x - first_point_x); //斜率
float AA = (c2*c2 + 1);
float BB = (2 * first_point_x*c2 - 2 * c1*c2 - 2 * first_point_y);
float CC = first_point_x*first_point_x - 2 * first_point_x*c1 + c1*c1 + first_point_y*first_point_y - distance*distance;
center_y = (-BB + My_Sqrt(BB*BB - 4 * AA*CC)) /(float)(2 * AA);
center_x = c1 - c2 * center_y;
//带入圆的标准方程,求出每一个x
*(imag_out+first_point_x+first_point_y*Imag_x) = 0;
supplementary_point = first_point_x;
for (int i = 1; i <= dy; i++)
{
temp_x = My_Sqrt(distance*distance - (first_point_y-i-center_y)*(first_point_y-i-center_y))+center_x;
if (temp_x < supplementary_point)
{
deviation = supplementary_point - temp_x;
for (int j = 0; j <= deviation; j++)
{
*(imag_out+temp_x+j+(first_point_y-i)*Imag_x) = 0;
}
}
else
{
deviation = temp_x - supplementary_point;
for (int j = 0; j <= deviation; j++)
{
*(imag_out+temp_x-j+(first_point_y-i)*Imag_x) = 0;
}
}
supplementary_point = temp_x;
}
}
int my_abs(int value)
{
if(value>=0) return value;
else return -value;
}
int limit_a_b(int x, int a, int b)
{
if(x<a) x = a;
if(x>b) x = b;
return x;
}
float My_Sqrt(float number)
{
long i;
float x, y;
const float f = 1.5F;
float my_sqrt_reciprocal;
x = number * 0.5F;
y = number;
i = * ( long * ) &y;
i = 0x5f3759df - ( i >> 1 );
y = * ( float * ) &i;
y = y * ( f - ( x * y * y ) );
y = y * ( f - ( x * y * y ) );
my_sqrt_reciprocal = y;
return number * my_sqrt_reciprocal;
}
/*
函数名称：int16 limit(int16 x, int16 y)
功能说明：求x,y中的最小值
参数说明：
函数返回：返回两值中的最小值
修改时间：2022年9月8日
备 注：
example： limit( x, y)
*/
int16 limit1(int x, int y)
{
if (x > y) return y;
else if (x < -y) return -y;
else return x;
}
/*变量声明*/
uint8 original_image[image_h][image_w];
uint8 image_thereshold;//图像分割阈值
//------------------------------------------------------------------------------------------------------------------
// @brief 获得一副灰度图像
// @since v1.0
//------------------------------------------------------------------------------------------------------------------
void Get_image(uint8(*mt9v03x_image)[image_w])
{
#define use_num 1 //1就是不压缩，2就是压缩一倍
uint8 i = 0, j = 0, row = 0, line = 0;
for (i = 0; i < image_h; i += use_num) //
{
for (j = 0; j <image_w; j += use_num) //
{
original_image[row][line] = mt9v03x_image[i][j];//这里的参数填写你的摄像头采集到的图像
line++;
}
line = 0;
row++;
}
}
//------------------------------------------------------------------------------------------------------------------
// @brief 动态阈值
// @since v1.0
//------------------------------------------------------------------------------------------------------------------
uint8 otsuThreshold(uint8 *image, int col, int row)
{
#define GrayScale 256
int Image_Width = col;
int Image_Height = row;
int X; int Y;
uint8* data = image;
int HistGram[GrayScale] = {0};
int Amount = 0;
int PixelBack = 0;
int PixelIntegralBack = 0;
int PixelIntegral = 0;
int32 PixelIntegralFore = 0;
int32 PixelFore = 0;
double OmegaBack=0, OmegaFore=0, MicroBack=0, MicroFore=0, SigmaB=0, Sigma=0; // 类间方差;
uint8 MinValue=0, MaxValue=0;
uint8 Threshold = 0;
for (Y = 0; Y <Image_Height; Y++) //Y<Image_Height改为Y =Image_Height；以便进行 行二值化
{
//Y=Image_Height;
for (X = 0; X < Image_Width; X++)
{
HistGram[(int)data[Y*Image_Width + X]]++; //统计每个灰度值的个数信息
}
}
for (MinValue = 0; MinValue < 256 && HistGram[MinValue] == 0; MinValue++) ; //获取最小灰度的值
for (MaxValue = 255; MaxValue > MinValue && HistGram[MinValue] == 0; MaxValue--) ; //获取最大灰度的值
if (MaxValue == MinValue)
{
return MaxValue; // 图像中只有一个颜色
}
if (MinValue + 1 == MaxValue)
{
return MinValue; // 图像中只有二个颜色
}
for (Y = MinValue; Y <= MaxValue; Y++)
{
Amount += HistGram[Y]; // 像素总数
}
PixelIntegral = 0;
for (Y = MinValue; Y <= MaxValue; Y++)
{
PixelIntegral += HistGram[Y] * Y;//灰度值总数
}
SigmaB = -1;
for (Y = MinValue; Y < MaxValue; Y++)
{
PixelBack = PixelBack + HistGram[Y]; //前景像素点数
PixelFore = Amount - PixelBack; //背景像素点数
OmegaBack = (double)PixelBack / Amount;//前景像素百分比
OmegaFore = (double)PixelFore / Amount;//背景像素百分比
PixelIntegralBack += HistGram[Y] * Y; //前景灰度值
PixelIntegralFore = PixelIntegral - PixelIntegralBack;//背景灰度值
MicroBack = (double)PixelIntegralBack / PixelBack;//前景灰度百分比
MicroFore = (double)PixelIntegralFore / PixelFore;//背景灰度百分比
Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);//g
if (Sigma > SigmaB)//遍历最大的类间方差g
{
SigmaB = Sigma;
Threshold = (uint8)Y;
}
}
return Threshold;
}
//------------------------------------------------------------------------------------------------------------------
// @brief 图像二值化，这里用的是大津法二值化。
// @since v1.0
//------------------------------------------------------------------------------------------------------------------
uint8 bin_image[image_h][image_w];//图像数组
void turn_to_bin(void)
{
uint8 i,j;
image_thereshold = otsuThreshold(mt9v03x_image[0], image_w, image_h);
for(i = 0;i<image_h;i++)
{
for(j = 0;j<image_w;j++)
{
if(mt9v03x_image[i][j]>image_thereshold)bin_image[i][j] = white_pixel;
else bin_image[i][j] = black_pixel;
}
}
}
/*
函数名称：void get_start_point(uint8 start_row)
功能说明：寻找两个边界的边界点作为八邻域循环的起始点
参数说明：输入任意行数
函数返回：无
修改时间：2022年9月8日
备 注：
example： get_start_point(image_h-2)
*/
uint8 start_point_l[2] = { 0 };//左边起点的x，y值
uint8 start_point_r[2] = { 0 };//右边起点的x，y值
uint8 get_start_point(uint8 start_row)
{
uint8 i = 0,l_found = 0,r_found = 0;
//清零
start_point_l[0] = 0;//x
start_point_l[1] = 0;//y
start_point_r[0] = 0;//x
start_point_r[1] = 0;//y
//从中间往左边，先找起点
for (i = image_w / 2; i > border_min; i--)
{
start_point_l[0] = i;//x
start_point_l[1] = start_row;//y
if (bin_image[start_row][i] == 255 && bin_image[start_row][i - 1] == 0)
{
//printf("找到左边起点image[%d][%d]\n", start_row,i);
l_found = 1;
break;
}
}
for (i = image_w / 2; i < border_max; i++)
{
start_point_r[0] = i;//x
start_point_r[1] = start_row;//y
if (bin_image[start_row][i] == 255 && bin_image[start_row][i + 1] == 0)
{
//printf("找到右边起点image[%d][%d]\n",start_row, i);
r_found = 1;
break;
}
}
if(l_found&&r_found)return 1;
else {
//printf("未找到起点\n");
return 0;
}
}
/*
函数名称：void search_l_r(int break_flag, uint8(*image)[image_w],int *l_stastic, int *r_stastic,
uint8 l_start_x, uint8 l_start_y, uint8 r_start_x, uint8 r_start_y,uint8*hightest)
功能说明：八邻域正式开始找右边点的函数，输入参数有点多，调用的时候不要漏了，这个是左右线一次性找完。
参数说明：
break_flag_r ：最多需要循环的次数
(*image)[image_w] ：需要进行找点的图像数组，必须是二值图,填入数组名称即可
特别注意，不要拿宏定义名字作为输入参数，否则数据可能无法传递过来
*l_stastic ：统计左边数据，用来输入初始数组成员的序号和取出循环次数
*r_stastic ：统计右边数据，用来输入初始数组成员的序号和取出循环次数
l_start_x ：左边起点横坐标
l_start_y ：左边起点纵坐标
r_start_x ：右边起点横坐标
r_start_y ：右边起点纵坐标
hightest ：循环结束所得到的最高高度
函数返回：无
修改时间：2022年9月25日
备 注：
example：
search_l_r((int)USE_num,image,&data_stastics_l, &data_stastics_r,start_point_l[0],
start_point_l[1], start_point_r[0], start_point_r[1],&hightest);
*/
#define USE_num image_h*3 //定义找点的数组成员个数按理说300个点能放下，但是有些特殊情况确实难顶，多定义了一点
//存放点的x，y坐标
int points_l[(int)USE_num][2] = { { 0 } };//左线
int points_r[(int)USE_num][2] = { { 0 } };//右线
int dir_r[(int)USE_num] = { 0 };//用来存储右边生长方向
int dir_l[(int)USE_num] = { 0 };//用来存储左边生长方向
int data_stastics_l = 0;//统计左边找到点的个数
int data_stastics_r = 0;//统计右边找到点的个数
uint8 hightest = 0;//最高点
uint8 l_lost_num,r_lost_num;
void search_l_r(int break_flag, uint8(*image)[image_w], int *l_stastic, int *r_stastic, uint8 l_start_x, uint8 l_start_y, uint8 r_start_x, uint8 r_start_y, uint8*hightest)
{
uint8 i = 0, j = 0;
//左边变量
uint8 search_filds_l[8][2] = { { 0 } };
uint8 index_l = 0;
uint8 temp_l[8][2] = { { 0 } };
uint8 center_point_l[2] = { 0 };
int l_data_statics;//统计左边
//定义八个邻域
static int8 seeds_l[8][2] = { {0, 1},{-1,1},{-1,0},{-1,-1},{0,-1},{1,-1},{1, 0},{1, 1}, };
//{-1,-1},{0,-1},{+1,-1},
//{-1, 0}, {+1, 0},
//{-1,+1},{0,+1},{+1,+1},
//这个是顺时针
//右边变量
uint8 search_filds_r[8][2] = { { 0 } };
uint8 center_point_r[2] = { 0 };//中心坐标点
uint8 index_r = 0;//索引下标
uint8 temp_r[8][2] = { { 0 } };
int r_data_statics;//统计右边
//定义八个邻域
static int8 seeds_r[8][2] = { {0, 1},{1,1},{1,0}, {1,-1},{0,-1},{-1,-1}, {-1, 0},{-1, 1}, };
//{-1,-1},{0,-1},{+1,-1},
//{-1, 0}, {+1, 0},
//{-1,+1},{0,+1},{+1,+1},
//这个是逆时针
l_data_statics = *l_stastic;//统计找到了多少个点，方便后续把点全部画出来
r_data_statics = *r_stastic;//统计找到了多少个点，方便后续把点全部画出来
//第一次更新坐标点 将找到的起点值传进来
center_point_l[0] = l_start_x;//x
center_point_l[1] = l_start_y;//y
center_point_r[0] = r_start_x;//x
center_point_r[1] = r_start_y;//y
//开启邻域循环
while (break_flag--)
{
//左边
for (i = 0; i < 8; i++)//传递8F坐标
{
search_filds_l[i][0] = center_point_l[0] + seeds_l[i][0];//x
search_filds_l[i][1] = center_point_l[1] + seeds_l[i][1];//y
}
//中心坐标点填充到已经找到的点内
points_l[l_data_statics][0] = center_point_l[0];//x
points_l[l_data_statics][1] = center_point_l[1];//y
l_data_statics++;//索引加一
//右边
for (i = 0; i < 8; i++)//传递8F坐标
{
search_filds_r[i][0] = center_point_r[0] + seeds_r[i][0];//x
search_filds_r[i][1] = center_point_r[1] + seeds_r[i][1];//y
}
//中心坐标点填充到已经找到的点内
points_r[r_data_statics][0] = center_point_r[0];//x
points_r[r_data_statics][1] = center_point_r[1];//y
index_l = 0;//先清零，后使用
for (i = 0; i < 8; i++)
{
temp_l[i][0] = 0;//先清零，后使用
temp_l[i][1] = 0;//先清零，后使用
}
//左边判断
for (i = 0; i < 8; i++)
{
if (image[search_filds_l[i][1]][search_filds_l[i][0]] == 0
&& image[search_filds_l[(i + 1) & 7][1]][search_filds_l[(i + 1) & 7][0]] == 255)
{
temp_l[index_l][0] = search_filds_l[(i)][0];
temp_l[index_l][1] = search_filds_l[(i)][1];
index_l++;
dir_l[l_data_statics - 1] = (i);//记录生长方向
}
if (index_l)
{
//更新坐标点
center_point_l[0] = temp_l[0][0];//x
center_point_l[1] = temp_l[0][1];//y
for (j = 0; j < index_l; j++)
{
if (center_point_l[1] > temp_l[j][1])
{
center_point_l[0] = temp_l[j][0];//x
center_point_l[1] = temp_l[j][1];//y
}
}
}
}
if ((points_r[r_data_statics][0]== points_r[r_data_statics-1][0]&& points_r[r_data_statics][0] == points_r[r_data_statics - 2][0]
&& points_r[r_data_statics][1] == points_r[r_data_statics - 1][1] && points_r[r_data_statics][1] == points_r[r_data_statics - 2][1])
||(points_l[l_data_statics-1][0] == points_l[l_data_statics - 2][0] && points_l[l_data_statics-1][0] == points_l[l_data_statics - 3][0]
&& points_l[l_data_statics-1][1] == points_l[l_data_statics - 2][1] && points_l[l_data_statics-1][1] == points_l[l_data_statics - 3][1]))
{
//printf("三次进入同一个点，退出\n");
break;
}
if (my_abs(points_r[r_data_statics][0] - points_l[l_data_statics - 1][0]) < 2
&& my_abs(points_r[r_data_statics][1] - points_l[l_data_statics - 1][1] < 2)
)
{
//printf("\n左右相遇退出\n");
*hightest = (points_r[r_data_statics][1] + points_l[l_data_statics - 1][1]) >> 1;//取出最高点
//printf("\n在y=%d处退出\n",*hightest);
break;
}
if ((points_r[r_data_statics][1] < points_l[l_data_statics - 1][1]))
{
// printf("\n如果左边比右边高了，左边等待右边\n");
continue;//如果左边比右边高了，左边等待右边
}
if (dir_l[l_data_statics - 1] == 7
&& (points_r[r_data_statics][1] > points_l[l_data_statics - 1][1]))//左边比右边高且已经向下生长了
{
//printf("\n左边开始向下了，等待右边，等待中... \n");
center_point_l[0] = points_l[l_data_statics - 1][0];//x
center_point_l[1] = points_l[l_data_statics - 1][1];//y
l_data_statics--;
}
r_data_statics++;//索引加一
index_r = 0;//先清零，后使用
for (i = 0; i < 8; i++)
{
temp_r[i][0] = 0;//先清零，后使用
temp_r[i][1] = 0;//先清零，后使用
}
//右边判断
for (i = 0; i < 8; i++)
{
if (image[search_filds_r[i][1]][search_filds_r[i][0]] == 0
&& image[search_filds_r[(i + 1) & 7][1]][search_filds_r[(i + 1) & 7][0]] == 255)
{
temp_r[index_r][0] = search_filds_r[(i)][0];
temp_r[index_r][1] = search_filds_r[(i)][1];
index_r++;//索引加一
dir_r[r_data_statics - 1] = (i);//记录生长方向
//printf("dir[%d]:%d\n", r_data_statics - 1, dir_r[r_data_statics - 1]);
}
if (index_r)
{
//更新坐标点
center_point_r[0] = temp_r[0][0];//x
center_point_r[1] = temp_r[0][1];//y
for (j = 0; j < index_r; j++)
{
if (center_point_r[1] > temp_r[j][1])
{
center_point_r[0] = temp_r[j][0];//x
center_point_r[1] = temp_r[j][1];//y
}
}
}
}
}
//取出循环次数
*l_stastic = l_data_statics;
*r_stastic = r_data_statics;
}
/*
函数名称：void get_left(int total_L)
功能说明：从八邻域边界里提取需要的边线
参数说明：
total_L ：找到的点的总数
函数返回：无
修改时间：2022年9月25日
备 注：
example： get_left(data_stastics_l );
*/
uint8 l_border[image_h];//左线数组
uint8 r_border[image_h];//右线数组
uint8 center_line[image_h];//中线数组
void get_left(int total_L)
{
uint8 i = 0;
int j = 0;
uint8 h = 0;
//初始化
for (i = 0;i<image_h;i++)
{
l_border[i] = border_min;
}
h = image_h - 2;
//左边
for (j = 0; j < total_L; j++)
{
//printf("%d\n", j);
if (points_l[j][1] == h)
{
l_border[h] = points_l[j][0]+1;
if(h>=40&&l_border[h]==2)l_lost_num++;
}
else continue; //每行只取一个点，没到下一行就不记录
h--;
if (h == 0)
{
break;//到最后一行退出
}
}
}
/*
函数名称：void get_right(int total_R)
功能说明：从八邻域边界里提取需要的边线
参数说明：
total_R ：找到的点的总数
函数返回：无
修改时间：2022年9月25日
备 注：
example：get_right(data_stastics_r);
*/
void get_right(int total_R)
{
uint8 i = 0;
int j = 0;
uint8 h = 0;
for (i = 0; i < image_h; i++)
{
r_border[i] = border_max;//右边线初始化放到最右边，左边线放到最左边，这样八邻域闭合区域外的中线就会在中间，不会干扰得到的数据
}
h = image_h - 2;
//右边
for (j = 0; j < total_R; j++)
{
if (points_r[j][1] == h)
{
r_border[h] = points_r[j][0] - 1;
if(h>=40&&r_border[h]==185)r_lost_num++;
}
else continue;//每行只取一个点，没到下一行就不记录
h--;
if (h == 0)break;//到最后一行退出
}
}
//定义膨胀和腐蚀的阈值区间
#define threshold_max 255*5//此参数可根据自己的需求调节
#define threshold_min 255*2//此参数可根据自己的需求调节
void image_filter(uint8(*bin_image)[image_w])//形态学滤波，简单来说就是膨胀和腐蚀的思想
{
int i, j;
int num = 0;
for (i = 1; i < image_h - 1; i++)
{
for (j = 1; j < (image_w - 1); j++)
{
//统计八个方向的像素值
num =
bin_image[i - 1][j - 1] + bin_image[i - 1][j] + bin_image[i - 1][j + 1]
+ bin_image[i][j - 1] + bin_image[i][j + 1]
+ bin_image[i + 1][j - 1] + bin_image[i + 1][j] + bin_image[i + 1][j + 1];
if (num >= threshold_max && bin_image[i][j] == 0)
{
bin_image[i][j] = 255;//白 可以搞成宏定义，方便更改
}
if (num <= threshold_min && bin_image[i][j] == 255)
{
bin_image[i][j] = 0;//黑
}
}
}
}
/*
函数名称：void image_draw_rectan(uint8(*image)[image_w])
功能说明：给图像画一个黑框
参数说明：uint8(*image)[image_w] 图像首地址
函数返回：无
修改时间：2022年9月8日
备 注：
example： image_draw_rectan(bin_image);
*/
void image_draw_rectan(uint8(*image)[image_w])
{
uint8 i = 0;
for (i = 0; i < image_h; i++)
{
image[i][0] = 0;
image[i][1] = 0;
image[i][image_w - 1] = 0;
image[i][image_w - 2] = 0;
}
for (i = 0; i < image_w; i++)
{
image[0][i] = 0;
image[1][i] = 0;
image[image_h-1][i] = 0;//环岛右下拐点单独突起时，边界断了，所以需要图像四个边界都有黑线。
}
}
/*
函数名称：void image_process(void)
功能说明：最终处理函数
参数说明：无
函数返回：无
修改时间：2022年9月8日
备 注：
example： image_process();
*/
void image_process(void)
{
int i;
uint8 hightest = 30;//定义一个最高行，tip：这里的最高指的是y值的最小
/*这是离线调试用的*/
//Get_image(mt9v03x_image);
turn_to_bin();
/*提取赛道边界*/
image_filter(bin_image);//滤波，更好的解决八邻域的误判
image_draw_rectan(bin_image);//预处理
//清零
data_stastics_l = 0;
data_stastics_r = 0;
l_lost_num=0;
r_lost_num=0;
if (get_start_point(image_h - 2))//找到起点了，再执行八领域，没找到就一直找
{
// printf("正在开始八领域\n");
search_l_r((int)USE_num, bin_image, &data_stastics_l, &data_stastics_r, start_point_l[0], start_point_l[1], start_point_r[0], start_point_r[1], &hightest);
// printf("八邻域已结束\n");
// 从爬取的边界线内提取边线 ， 这个才是最终有用的边线
get_left(data_stastics_l);
get_right(data_stastics_r);
//处理函数放这里，不要放到if外面去了，不要放到if外面去了，不要放到if外面去了，重要的事说三遍
}
//cross_fill(bin_image,l_border,r_border,200,200,dir_l,dir_r,points_l,points_r);
zebracount=0,k1=3,k2=-3;
break_1[0]=0,break_1[1]=0,break_2[0]=0,break_2[1]=0,break_3[0]=0,break_3[1]=0,break_4[0]=0,break_4[1]=0,break_5[0]=0,break_5[1]=0;
if(huandaozuoflag1==0&&huandaozuoflag==0&&bizhangflag1==0&&huandaoflag1==0&&huandaoflag2==0&&huandaoflag3==0&&huandaoflag4==0&&huandaoflag5==0&&huandaoflag6==0){
crossing22();
//zebra();
}
if(crossflag1==0&&cross2flag1==0&&cross2flag2==0&&bizhangflag1==0&&huandaozuoflag1==0&&huandaozuoflag==0){
huandaoyou();
//zebra();
}
if(crossflag1==0&&cross2flag1==0&&cross2flag2==0&&bizhangflag1==0&&huandaoflag1==0&&huandaoflag2==0&&huandaoflag3==0&&huandaoflag4==0&&huandaoflag5==0&&huandaoflag6==0){
huandaozuo();
}
if(podaoflag3==1&&crossflag1==0&&cross2flag1==0&&cross2flag2==0&&huandaozuoflag==0&&huandaozuoflag1==0&&huandaoflag1==0&&huandaoflag2==0&&huandaoflag3==0&&huandaoflag4==0&&huandaoflag5==0&&huandaoflag6==0){
bizhang();
}
if(bizhangflag1==0&&podaoflag3==0&&crossflag1==0&&cross2flag1==0&&cross2flag2==0&&bizhangflag1==0&&huandaozuoflag1==0&&huandaozuoflag==0&&huandaoflag1==0&&huandaoflag2==0&&huandaoflag3==0&&huandaoflag4==0&&huandaoflag5==0&&huandaoflag6==0){
podao();
}
//Draw_Curve(bin_image[0], 10, 10,100, 100,100);
//显示图像 改成你自己的就行 等后期足够自信了，显示关掉，显示屏挺占资源的
ips200_displayimage03x((const uint8 *)bin_image, MT9V03X_W, MT9V03X_H);
//根据最终循环次数画出边界点
for (i = 0; i < data_stastics_l; i++)
{
ips200_draw_point(points_l[i][0]+2, points_l[i][1], uesr_BLUE);//显示起点
}
for (i = 0; i < data_stastics_r; i++)
{
ips200_draw_point(points_r[i][0]-2, points_r[i][1], uesr_RED);//显示起点
}
for (i = hightest; i < image_h-1; i++)
{
center_line[i] = (l_border[i] + r_border[i]) >> 1;//求中线
//求中线最好最后求，不管是补线还是做状态机，全程最好使用一组边线，中线最后求出，不能干扰最后的输出
//当然也有多组边线的找法，但是个人感觉很繁琐，不建议
ips200_draw_point(center_line[i], i, RGB565_PINK);//显示起点 显示中线
ips200_draw_point(l_border[i], i, uesr_GREEN);//显示起点 显示左边线//这里不要改会报图像越界屏幕的错
ips200_draw_point(r_border[i], i, uesr_GREEN);//显示起点 显示右边线//这里不要改会报图像越界屏幕的错
}
road=center_line[110]-94;
}
/*
这里是起点（0.0）***************——>*************x值最大
************************************************************
************************************************************
************************************************************
************************************************************
******************假如这是一副图像*************************
***********************************************************
***********************************************************
***********************************************************
***********************************************************
***********************************************************
***********************************************************
y值最大*******************************************(188.120)
*/
/**
* @brief 最小二乘法
* @param uint8 begin 输入起点
* @param uint8 end 输入终点
* @param uint8 *border 输入需要计算斜率的边界首地址
* @see CTest Slope_Calculate(start, end, border);//斜率
* @return 返回说明
* -<em>false</em> fail
* -<em>true</em> succeed
*/
float Slope_Calculate(uint8 begin, uint8 end, uint8 *border)
{
float xsum = 0, ysum = 0, xysum = 0, x2sum = 0;
int16 i = 0;
float result = 0;
static float resultlast;
for (i = begin; i < end; i++)
{
xsum += i;
ysum += border[i];
xysum += i * (border[i]);
x2sum += i * i;
}
if ((end - begin)*x2sum - xsum * xsum) //判断除数是否为零
{
result = ((end - begin)*xysum - xsum * ysum) / ((end - begin)*x2sum - xsum * xsum);
resultlast = result;
}
else
{
result = resultlast;
}
return result;
}
/**
* @brief 计算斜率截距
* @param uint8 start 输入起点
* @param uint8 end 输入终点
* @param uint8 *border 输入需要计算斜率的边界
* @param float *slope_rate 输入斜率地址
* @param float *intercept 输入截距地址
* @see CTest calculate_s_i(start, end, r_border, &slope_l_rate, &intercept_l);
* @return 返回说明
* -<em>false</em> fail
* -<em>true</em> succeed
*/
void calculate_s_i(uint8 start, uint8 end, uint8 *border, float *slope_rate, float *intercept)
{
int i, num = 0;
int xsum = 0, ysum = 0;
float y_average, x_average;
start = limit_a_b(start, 0, image_h);
end = limit_a_b(end, 0, image_h);
num = 0;
xsum = 0;
ysum = 0;
y_average = 0;
x_average = 0;
for (i = start; i < end; i++)
{
xsum += i;
ysum += border[i];
num++;
}
//计算各个平均数
if (num)
{
x_average = (float)(xsum / num);
y_average = (float)(ysum / num);
}
/*计算斜率*/
*slope_rate = Slope_Calculate(start, end, border);//斜率
*intercept = y_average - (*slope_rate)*x_average;//截距
}
/**
* @brief 十字补线函数
* @param uint8(*image)[image_w] 输入二值图像
* @param uint8 *l_border 输入左边界首地址
* @param uint8 *r_border 输入右边界首地址
* @param int total_num_l 输入左边循环总次数
* @param int total_num_r 输入右边循环总次数
* @param int *dir_l 输入左边生长方向首地址
* @param int *dir_r 输入右边生长方向首地址
* @param int(*points_l)[2] 输入左边轮廓首地址
* @param int(*points_r)[2] 输入右边轮廓首地址
* @see CTest cross_fill(image,l_border, r_border, data_statics_l, data_statics_r, dir_l, dir_r, points_l, points_r);
* @return 返回说明
* -<em>false</em> fail
* -<em>true</em> succeed
*/
void cross_fill(uint8(*image)[image_w], uint8 *l_border, uint8 *r_border, int total_num_l, int total_num_r,
int *dir_l, int *dir_r, int(*points_l)[2], int(*points_r)[2])
{
uint8 i;
uint8 break_num_l = 0;
uint8 break_num_r = 0;
uint8 start, end;
// float slope_l_rate2 = 0, intercept_l2 = 0;
//出十字
for (i = 1; i < total_num_l; i++)
{
if (dir_l[i - 1] == 4 && dir_l[i] == 4 && dir_l[i + 3] == 6 && dir_l[i + 5] == 6 && dir_l[i + 7] == 6)
{
break_num_l = points_l[i][1];//传递y坐标
// printf("brea_knum-L:%d\n", break_num_l);
// printf("I:%d\n", i);
// printf("十字标志位：1\n");
break;
}
}
for (i = 1; i < total_num_r; i++)
{
if (dir_r[i - 1] == 4 && dir_r[i] == 4 && dir_r[i + 3] == 6 && dir_r[i + 5] == 6 && dir_r[i + 7] == 6)
{
break_num_r = points_r[i][1];//传递y坐标
// printf("brea_knum-R:%d\n", break_num_r);
// printf("I:%d\n", i);
// printf("十字标志位：1\n");
break;
}
}
if (break_num_l&&break_num_r&&image[image_h - 1][4] && image[image_h - 1][image_w - 4])//两边生长方向都符合条件
{
//计算斜率
start = break_num_l - 15;
start = limit_a_b(start, 0, image_h);
end = break_num_l - 5;
calculate_s_i(start, end, l_border, &slope_l_rate, &intercept_l);
//printf("slope_l_rate:%d\nintercept_l:%d\n", slope_l_rate, intercept_l);
for (i = break_num_l - 5; i < image_h - 1; i++)
{
l_border[i] = slope_l_rate * (i)+intercept_l;//y = kx+b
l_border[i] = limit_a_b(l_border[i], border_min, border_max);//限幅
}
//计算斜率
start = break_num_r - 15;//起点
start = limit_a_b(start, 0, image_h);//限幅
end = break_num_r - 5;//终点
calculate_s_i(start, end, r_border, &slope_l_rate, &intercept_l);
//printf("slope_l_rate:%d\nintercept_l:%d\n", slope_l_rate, intercept_l);
for (i = break_num_r - 5; i < image_h - 1; i++)
{
r_border[i] = slope_l_rate * (i)+intercept_l;
r_border[i] = limit_a_b(r_border[i], border_min, border_max);
}
}
}
uint8 huandaoflag1,huandaoflag2,huandaoflag2_1,huandaoflag3,huandaoflag4,huandaoflag5,huandaoflag6;
uint8 break_1[2],break_2[2],break_3[2];
float slope_l_rate = 0, intercept_l = 0;
float k1,k2;
void huandaoyou(){
uint8 break_num_r = 0;
uint8 start, end;
break_1[0]=0,break_1[1]=0,break_2[0]=0,break_2[1]=0,break_3[0]=0,break_3[1]=0;
// float slope_l_rate = 0, intercept_l = 0;
if(l_lost_num<=10&&r_lost_num>=40){
calculate_s_i( 70, 90, l_border, &slope_l_rate, &intercept_l);
k1=slope_l_rate;
calculate_s_i( 90, 110, l_border, &slope_l_rate, &intercept_l);
k2=slope_l_rate;
if(k2-k1<=0.1&&k2-k1>=-0.1){
for(int i=118;i>80;i--){
if(r_border[i]>=110&&r_border[i]<=180&&r_border[i-5]==185&&huandaoflag2==0&&huandaoflag3==0&&huandaoflag4==0&&huandaoflag5==0&&huandaoflag6==0){
huandaoflag1=1;
huandaoflag2=1;
break_1[1]=i;//[0]表示列数，[1]表示行数
break_1[0]=r_border[i];
// calculate_s_i( 80, 100, l_border, &slope_l_rate, &intercept_l);
break;
}
else{
huandaoflag1=0;
}
}
}
}
if(huandaoflag2==1){
int min=140;
for (int i = 40; i < 80; i+=2)
{
int j=94;
for(j=94;j<150&&break_2[0]==0;j+=2){
if(bin_image[i][j]==white_pixel&&bin_image[i][j+2]==black_pixel){//找右中拐点，直走拉线
break_2[0]=j;
break_2[1]=i;
break;}
}
// min=fmin(j,min);
}
calculate_s_i( 80, 100, l_border, &slope_l_rate, &intercept_l);
if(break_2[0]!=0&&break_2[1]!=0){
for (int i = break_2[1]; i < image_h - 1; i++)
{
r_border[i] = break_2[0]-(slope_l_rate) * (i-break_2[1]);
r_border[i] = limit_a_b(r_border[i], border_min, border_max);
}}
}
if(huandaoflag2==1&&r_lost_num<=20&&r_border[115]!=185&&r_border[110]!=185&&r_border[105]!=185&&r_border[100]!=185){
huandaoflag2=0;
huandaoflag2_1=1;}
if(huandaoflag2_1==1&&(r_border[80]==185||r_border[100]==185)){
huandaoflag3=1;
huandaoflag2_1=0;
}
if(huandaoflag3==1&&huandaoflag1==0&&huandaoflag2==0){
// for(int i=40;i<100;i++){
// if(r_border[i]<=170&&r_border[i+10]>=185){//找右上拐点，拐弯拉线
int more_write=0;
for(int k=70;k<150;k+=3){if(bin_image[i+10][k]==black_pixel){more_write=1;break;}}
if(more_write==0){//中心全为白点
// break_3[0]=r_border[i];
// break_3[1]=i;
// break;
}
// }
// }
for(int i=116;i>=40;i--){
for(int j=5;j<=180;j++){
if(bin_image[i][j]==white_pixel&&bin_image[i][j+2]==black_pixel&&bin_image[i][j+20]==white_pixel&&j>=l_border[i]&&j<r_border[i]-5){
break_3[0]=j;
break_3[1]=i;
break;
}
}
if(break_3[0]!=0) break;
}
if(break_3[0]!=0&&break_3[1]!=0){
slope_l_rate=(break_3[1]-118-0.0)/(break_3[0]-0-0.0);
for(int i=118;i>break_3[1];i--){//内圈都丢线了，寻内线感觉效果不行
// l_border[i]=r_border[i]-90;
l_border[i]= break_3[0]+40+(i-break_3[1]-0.0)/slope_l_rate;//转弯角度不够，人为把l_border增大。
l_border[i] = limit_a_b(l_border[i], border_min, border_max);
}
}
}
if(huandaoflag3==1&&huandaoflag2==0&&huandaoflag1==0&&l_lost_num==0&&l_border[40]==1&&r_border[40]==186&&((l_border[60]==1&&r_border[60]==186)||(l_border[60]>=110&&r_border[60]>=180))){//环内
// int whites;
// for(int k=3;k<185;k++){
// if(bin_image[60][k]==white_pixel){
// whites++;
// }
// }
// if(whites<50){
huandaoflag3=0;//环内
huandaoflag4=1;
// }
}
if(huandaoflag4==1&&huandaoflag3==0){//刚出环拉线拉直线
if((l_border[60]==2&&r_border[60]==185)||(l_border[80]==2&&r_border[80]==185)){
huandaoflag4=0;
huandaoflag5=1;
}
}
if(huandaoflag5==1&&huandaoflag4==0){
slope_l_rate=(60+0.0)/(170+0.0);
for(int i=118;i>60;i--){//刚出环拉线拉直线
// l_border[i]=r_border[i]-90;
l_border[i]= 185-(i-60-0.0)/slope_l_rate;
l_border[i] = limit_a_b(l_border[i], border_min, border_max);
}
}
if(huandaoflag5==1&&l_lost_num<=10){
huandaoflag5=0;
huandaoflag6=1;
}
if(huandaoflag6==1&&huandaoflag5==0){
for(int i=50;i<110;i++){
if(r_border[i]<=160&&r_border[i+5]>=185){//再找右上拐点，直走拉线
// int more_write=0;
// for(int k=80;k<170;k+=3){if(bin_image[i+10][k]==black_pixel){more_write=1;break;}}
// if(more_write==0){//中心全为白点
break_3[0]=r_border[i];
break_3[1]=i;
break;
// }
}
}
if(break_3[0]!=0&&break_3[1]!=0){
// slope_l_rate=(break_3[1]-118-0.0)/(break_3[0]-l_border[100]-0.0);
calculate_s_i(break_3[1]-5, break_3[1], r_border, &slope_l_rate, &intercept_l);
for(int i=118;i>break_3[1];i--){//内圈都丢线了，寻内线感觉效果不行
// l_border[i]=r_border[i]-90;
// r_border[i]= break_3[0]+(slope_l_rate) * (i-break_3[1]);
r_border[i] = slope_l_rate * (i)+intercept_l;
r_border[i] = limit_a_b(r_border[i], border_min, border_max);
}
}
}
if(huandaoflag6==1&&l_lost_num==0&&r_lost_num<=10&&l_border[113]<185&&l_border[110]<185&&r_border[105]<185){
huandaoflag6=0;
}
}
八领域处理算法
最新推荐文章于 2024-08-31 20:59:22 发布
2253
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
