智能车复工日记【3】：图像处理——基本扫线和基本特征提取和十字补线
目录
- 前言
- 基本扫线（除了进入环岛状态或者坡道或者十字路口的普通扫线）
- 基本数据和初步特征
- 进一步特征提取
- 1、计算并且显示前n行左右线各丢失数目（不break和break的都有）
- 2、计算左右线方差（以右线为例）
- 【a】计算右线曲率（选三个点：r_start、中点、break点）
- 【b】如果右线曲率在一定的范围，就从空白行开始进行右线拟合,从空白行开始计算斜率,否则则从0行开始拟合和计算
- 3、对初步扫出的线进行分析连续性（对左右线）
- 4、计算判断环岛需要的参数（在坡道状态时不进行此部分运算）
- 【A1】在2-50行内找右下拐点
- 【A2】在2-50行内找左下拐点
- 【B1】找右中拐点,分为两种情况，一种右下找到了，一种是右下没找到,右下找到的话就从右下开始往上找，右下没找的话就从8行开始往上找
- 【B2】找左中拐点,分为两种情况，一种左下找到了，一种是左下没找到,左下找到的话就从左下开始往上找，左下没找的话就从8行开始往上找
- 【C1】找右上拐点。一种右中找到了，一种是右中没找到,右中找到的话就从右中开始往上找，右中没找的话就从25行开始往上找
- 【C2】找左上拐点。一种左中找到了，一种是左中没找到,找到的话就从左中开始往上找，左中没找的话就从25行开始往上找
- 5、计算判断十字需要的参数（在坡道或环岛状态时不进行此部分运算）
- 【A】初步找四个拐点（上拐点部分情况下是不准的）
- 【B】分情况讨论：1、入十字之前，用下拐点和上拐点拉2、入十字了，用两个上拐点拉下来
- 【C1】入十字之前的操作
- 【C2】入十字后的操作
- 6、中线断线补线（补breakhang之后的线，防止breakhang过低，取PID得点取到虚空处）
- 7、计算40行以内的中线的偏差和
- 8、计算40行以内的中线的最大值和最小值并且计算出两者之差
- 9、对初步扫出的线进行分析连续性（对中线）
- 10、保存三线值，并将这一帧的第6行中线值保存为old，用到下一帧。
- 总结
前言
图像大小185*70，通过扫线获取左右两边数据，然后左加右除以二得到中线值。(降维处理)
基本扫线（除了进入环岛状态或者坡道或者十字路口的普通扫线）
基本思路：从下往上扫，从中间往两边扫。上一行的中线值记录下来，作为这一行往两边扫的起始值
需要注意的点：
1、判断边界的条件：连续有两个是黑，则判断较远的为边界，此时就需要注意数组越界的问题。
2、边界数组大小为70，和图像的高一样，意味着，每行只能有一个边界，所以当出现下面情况，边界就会不完整。
3、当这一行的中线是黑的且下一行同一列也是黑的，退出循环，表明后面的数据不进行采集。（这说明已经采集到的赛道外面了）。并且为了防止提前断线，我们限制二十行以下不进行break处理。
for (j = 0; j < 70; j++)
{
for (i = (byte)old; i >= 0 && i < 184; i++)
{
fiv_width[j]++;
if (Pixels[j, i + 1] == 0 && Pixels[j, i] == 0)
{
leftfindflag[j] = 1;
lefetline[j] = (byte)(i + 1);
break;
}
}
for (i = (byte)old; i <= 185 && i > 1; i--)
{
fiv_width[j]++;
if (Pixels[j, i - 1] == 0 && Pixels[j, i] == 0)
{
rightfindflag[j] = 1;
rightline[j] = (byte)(i - 1);
break;
}
}
if (j <= 50 && leftfindflag[j] == 1 && rightfindflag[j] == 1) flag_s++;
if (j <= 25 && leftfindflag[j] == 0 && rightfindflag[j] == 0) times++;
if (j >= 20 && (lefetline[j] - lefetline[j - 1]) <= -20) leftline_duan_dian = j;
if (j >= 20 && (rightline[j] - rightline[j - 1]) >= 20) rightline_duan_dian = j;
centerline[j] = (byte)((lefetline[j] + rightline[j]) / 2);
if ((Pixels[j, centerline[j]]) == 0 && (Pixels[j + 1, centerline[j]]) == 0)
{
break_hangshu = j;
//last_break_hangshu = break_hangshu;
//也就是说二十行一下是不会break的
if (break_hangshu >= 20) //防止在一开始就break
{
break;
}
}
old = centerline[j];
}
SetText("在多少行break（初次扫线）： " + break_hangshu);
SetText("rightline_duan_dian" + rightline_duan_dian);
SetText("leftline_duan_dian" + leftline_duan_dian);
old = centerline[5]; //初次扫线完毕，将old重新赋值
基本数据和初步特征
现在来看看我们初步基本扫线我们收集到的基本数据：
1、左线值：lefetline
2、右线值：rightline
3、中线值：centerline
4、每行赛道宽度：fiv_width
5、每行左线点是否扫到：leftfindflag
6、每行右线点是否扫到：rightfindflag
7、中线断开的目标行：break_hangshu
8、50行以内两边都扫到的次数：flag_s
9、25行以内两边全丢的次数：times
10、20行以上左线突然性断裂的目标行：leftline_duan_dian
11、20行以上右线突然性断裂的目标行：rightline_duan_dian
观察可得，其实最主要的数据藏在三条线数组中，其他的特征都是围绕着三线的，接下来我们进一步提取特征。
进一步特征提取
1、计算并且显示前n行左右线各丢失数目（不break和break的都有）
不break的是lost_times,break的是起始空白行r_start（l_start）
lostleft_times：前n行左线未扫到的次数
lostright_times：前n行右线未扫到的次数
(这里和上面的初步提取有些许重复之处)
l_start：在l_start以下的左线全是未扫到
r_start：在r_start以下的右线线全是未扫到
形象看，其实就是这两个点：
void Cal_losttimes(int times)
{
byte i;
byte flag_of_rightbreak = 0;
byte flag_of_leftbreak = 0;
for (i = 0; i < times; i++)
{
//左线操作
if (leftfindflag[i] == 0) //未扫到线
{
lostleft_times++;
if (flag_of_leftbreak == 0) //如果在这一行之前没有遭遇断线，则计数
{
l_start++;
}
}
else //扫到线
{
//lostleft_times不继续增加
flag_of_leftbreak = 1; //break标志成立
}
//右线操作
if (rightfindflag[i] == 0) //未扫到线
{
lostright_times++;
if (flag_of_rightbreak == 0) //如果在这一行之前没有遭遇断线，则计数
{
r_start++;
}
}
else //扫到线
{
//lostright_times不继续增加
flag_of_rightbreak = 1; //break标志成立
}
}
SetText(" lostleft_times " + lostleft_times);
SetText(" lostright_times " + lostright_times);
SetText("L_start: " + l_start);
SetText("R_start: " + r_start);
}
2、计算左右线方差（以右线为例）
【a】计算右线曲率（选三个点：r_start、中点、break点）
byte curvity_point1 = (byte)((r_start + break_hangshu) / 2); //中点
byte curvity_point2 = 0;
if (break_hangshu >= 5)
{
curvity_point2 = (byte)(break_hangshu - 3);
}
else
{
curvity_point2 = (byte)(break_hangshu);
}
curvity_right = process_curvity(rightline[r_start], r_start, rightline[curvity_point1], curvity_point1, rightline[curvity_point2], curvity_point2);
【b】如果右线曲率在一定的范围，就从空白行开始进行右线拟合,从空白行开始计算斜率,否则则从0行开始拟合和计算
//曲率接近0说明线挺直的，为了体现直线方差小的特点，从第start行开始计算
if (curvity_right > -0.4 && curvity_right < 0.1 && r_start <= 32 && (break_hangshu - r_start) >= 7)
{
regression(2, r_start, break_hangshu); //拟合右线
k_right = parameterB;
SetText("右线斜率：" + k_right);
Cal_Line(k_right, parameterA,0, break_hangshu, FORE_RIGHT);
for (i = r_start; i < break_hangshu - 5; i++)
{
rou_of_right += (forecast_rightline[i] - rightline[i]) * (forecast_rightline[i] - rightline[i]);
}
}
//否则说明边界是曲线，此时为了凸显出曲线的方差大的特点，从第0行开始计算
else
{
regression(2, 0, break_hangshu); //拟合右线
k_right = parameterB;
SetText("右线斜率：" + k_right);
Cal_Line(k_right, parameterA, 0, break_hangshu,FORE_RIGHT);
for (i = 0; i < break_hangshu - 5; i++)
{
rou_of_right += (forecast_rightline[i] - rightline[i]) * (forecast_rightline[i] - rightline[i]);
}
}
3、对初步扫出的线进行分析连续性（对左右线）
参数：起始点，结束点，正阈值。负阈值。
juge_lineContinuity(10,60,5,-5,LEFT);
SetText(“左线long_turn_flag:” +long_turn_flag_left);
//10-60行左线这一行减去上一行在-5到+5范围内则认为是连续，否则记录break的点。
void juge_lineContinuity(byte start_point, byte end_point,int positive_T,int negatie_T,byte ArryName)
{
byte j;
switch (ArryName)
{
case 0: //CENTER
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
center_delta = centerline[j + 1] - centerline[j]; //left线的偏差
if (center_delta >= positive_T || center_delta <= negatie_T)
{
long_turn_flag = j;
break;
}
else
{
if (j >= end_point - 1)
{
long_turn_flag = end_point;
}
}
}
}
break;
case 1: //LEFT
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
left_delta = lefetline[j + 1] - lefetline[j]; //left线的偏差
if (left_delta >= positive_T || left_delta <= negatie_T)
{
long_turn_flag_left = j;
break;
}
else
{
if (j>= end_point-1)
{
long_turn_flag_left = end_point;
}
}
}
}
break;
case 2: //RIGHT
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
right_delta = rightline[j + 1] - rightline[j]; //left线的偏差
if (right_delta >= positive_T || right_delta <= negatie_T)
{
long_turn_flag_right = j;
break;
}
else
{
if (j >= end_point - 1)
{
long_turn_flag_right = end_point;
}
}
}
}
break;
default:
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
center_delta = centerline[j + 1] - centerline[j]; //left线的偏差
//大于偏差，break，记录break点
if (center_delta >= positive_T || center_delta <= negatie_T)
{
long_turn_flag = j;
break;
}
//小于偏差，若是大于end点，则记录end点，否则不做操作
else
{
if (j >= end_point - 1)
{
long_turn_flag = end_point;
}
}
}
}
break;
}
}
4、计算判断环岛需要的参数（在坡道状态时不进行此部分运算）
【A1】在2-50行内找右下拐点
【A2】在2-50行内找左下拐点
【B1】找右中拐点,分为两种情况，一种右下找到了，一种是右下没找到,右下找到的话就从右下开始往上找，右下没找的话就从8行开始往上找
【B2】找左中拐点,分为两种情况，一种左下找到了，一种是左下没找到,左下找到的话就从左下开始往上找，左下没找的话就从8行开始往上找
【C1】找右上拐点。一种右中找到了，一种是右中没找到,右中找到的话就从右中开始往上找，右中没找的话就从25行开始往上找
【C2】找左上拐点。一种左中找到了，一种是左中没找到,找到的话就从左中开始往上找，左中没找的话就从25行开始往上找
if (podao_flag == 0)
{
/***【A1】找右下拐点***********/
find_rightdown_point(2, 50, ROUNDISLAND);
SetText("右下拐点：" + right_turn_down[0] + " " + right_turn_down[1]);
/***【A2】找左下拐点***********/
find_leftdown_point(2, 50, ROUNDISLAND);
SetText("左下拐点：" + left_turn_down[0] + " " + left_turn_down[1]);
//【B1】找右中拐点,分为两种情况，一种右下找到了，一种是右下没找到,右下找到的话就从右下开始往上找，右下没找的话就从8行开始往上找
if (flag_find_huan_rightdown_point == 1 && huandao_memory != 5)
{
SetText("从右下开始网上找右中");
find_rightmiddle_point((byte)(right_turn_down[0] + 5), 60);
}
else
{
SetText("从第八行开始网上找右中");
find_rightmiddle_point(8,60);
}
if (right_turn_middle[0] != 0 && huandao_memory != 4) flag_find_huan_rightmiddle_point = 1;
SetText("右中拐点：" + right_turn_middle[0] + " " + right_turn_middle[1]);
//【B2】找左中拐点,分为两种情况，一种左下找到了，一种是左下没找到,左下找到的话就从左下开始往上找，左下没找的话就从8行开始往上找
if (flag_find_huan_leftdown_point == 1 && huandao_memory != 5 && huandao_memory != 3)
{
SetText("从左下开始往上找左中");
find_leftmiddle_point((byte)(left_turn_down[0] + 5), 60);
}
else
{
SetText("从第八行开始网上找左中");
find_leftmiddle_point(8, 60);
}
if (left_turn_middle[0] != 0 && huandao_memory != 4) flag_find_huan_leftmiddle_point = 1;
SetText("左中拐点：" + left_turn_middle[0] + " " + left_turn_middle[1]);
//【C1】找右上拐点。前提是中拐点一定存在！！
if (flag_find_huan_rightmiddle_point == 1 && right_turn_middle[0] < 45 && huandao_memory != 4 && huandao_memory != 5 && huandao_memory != 7 && huandao_memory != 8) //右中拐点存在
{
SetText("从右中开始往上找");
find_rightup_point((byte)(right_turn_middle[0] + 5), 62, ROUNDISLAND);
}
else
{
SetText("从第25行开始往上找");
find_rightup_point(25, 62, ROUNDISLAND);
}
//【C2】找左上拐点。前提是中拐点一定存在！
if (flag_find_huan_leftmiddle_point == 1 && left_turn_middle[0] < 45 && huandao_memory != 4 && huandao_memory != 5 && huandao_memory != 7 && huandao_memory != 8) //左中拐点存在
{
SetText("从左中开始往上找");
find_leftup_point((byte)(left_turn_middle[0] + 5), 62, ROUNDISLAND);
}
else
{
SetText("从第25行开始往上找");
find_leftup_point(25, 62, ROUNDISLAND);
}
SetText("右上拐点：" + right_turn_up[0] + " " + right_turn_up[1]);
SetText("左上拐点：" + left_turn_up[0] + " " + left_turn_up[1]);
}
5、计算判断十字需要的参数（在坡道或环岛状态时不进行此部分运算）
【A】初步找四个拐点（上拐点部分情况下是不准的）
【B】分情况讨论：1、入十字之前，用下拐点和上拐点拉2、入十字了，用两个上拐点拉下来
这里将times <6时判断为 入十字前 ，
将times > 6 && (l_start > 15 && r_start > 15 && My_Abs(l_start - r_start) <= 8时判断为 入十字后。
【C1】入十字之前的操作
【1】：如果找到两个下拐点，求出两处线的趋势。
【2】：如果趋势相同说明是弯道，此时将拐点记录为弯道拐点，以后判断问号弯时需要用到。
【3】：当左下拐点存在且（趋势不同或右边丢失数>25）认为左下拐点是真的拐点。
【4】：右下拐点同样如此推断。
【5】：当找到左下或右下拐点后，拟合并预测中线（通过对两个下拐点的确认程度可以判断出是左斜入十字、右斜入十字、正入十字），然后再顺着预测后的中线往两边重新扫线。（此时的扫线不需要对times和flag_s进行计数，只对三线数组和break行数进行覆盖操作，以及边线flag数组的先清零后覆盖的操作，注意如果此时的break行数比原来的低，则原本的高于break行数的三线数据将会保存）
【6】：对新扫出来的边界进行寻找上拐点操作（由于第一次扫线有误，所以上拐点位置大概率是错误的，需要重新检测）
【7】：开始补线（由于小s误判原因，这里加入限制：假如小s的flag为1就不进行补线操作，防止出错）
【8】：直到确认了要补线，我才将十字flag置1，也就是说此时才真正确认是十字状态
【9】：左右两边同时操作，同时找到上下拐点，用上下拐点附近的值拟合k和b，然后补线。只找到下拐点，则用下拐点的附近的值拟合k和b，然后补线
【10】：将补完的线存入双线数组，并且求出中线值
【C2】入十字后的操作
【1】：因为入十字了有许多空白行，会导致中线不准，沿中线往两边扫描得到的拐点也会不准，所以需要重新扫线，此时选择固定行扫线，且times和flag_s不再计数
【2】：重新找上拐点，且找到的上拐点必须大于原本的下拐点
【3】：如果上拐点存在，则进行补线。然后将十字flag置1
if (huandao_flag == 0 && podao_flag == 0)//不是环岛
{
/****【A】初步找拐点（必然是不准的，需要滤波）****/
/***【a1】找左下拐点***********/
find_leftdown_point(2,40, CROSSROAD);
/***【a2】找左上拐点***********/
find_leftup_point(5, 50, CROSSROAD);
/***【a3】找右下拐点***********/
find_rightdown_point(2, 40, CROSSROAD);
/***【a4】找右上拐点***********/
find_rightup_point(5, 50, CROSSROAD);
/**********显示出初步找到的拐点****************/
SetText("左下拐点 " + "左上拐点 " + "右下拐点 " + "右上拐点 ");
SetText(" " + left_turn_down[0] + " " + left_turn_down[1] + " " + left_turn_up[0] + " " + left_turn_up[1] + " " + right_turn_down[0] + " " + right_turn_down[1] + " " + right_turn_up[0] + " " + right_turn_up[1] + " ");
/**************【B】分情况讨论*****************/
/********【b1】入十字之前，用下面的点和上面的点拉************/
if (times <= 6)
{
//如果找到左下或者右下拐点，此时检测左右线趋势
float trend_of_left = 0;
float trend_of_right = 0;
//时时注意拐点坐标第一个是行数+1，处理时要做减一处理
if (left_turn_down[0] != 0 || right_turn_down[0] != 0)
{
if (left_turn_down[0] != 0 && right_turn_down[0] == 0)//左下拐点存在而右下拐点不存在
{
regression(1, left_turn_down[0] - 3, left_turn_down[0] + 2);//左线
trend_of_left = parameterB;
regression(2, left_turn_down[0] - 3, left_turn_down[0] + 2);//右线
trend_of_right = parameterB;
}
else if (right_turn_down[0] != 0 && left_turn_down[0] == 0) //右下拐点存在而左下拐点不存在
{
regression(1, right_turn_down[0] - 3, right_turn_down[0] + 2);//左线
trend_of_left = parameterB;
regression(2, right_turn_down[0] - 3, right_turn_down[0] + 2);//右线
trend_of_right = parameterB;
}
else if (left_turn_down[0] != 0 && left_turn_down[0] != 0) //左右拐点均存在
{
regression(1, left_turn_down[0] - 3, left_turn_down[0] + 2);//左线
trend_of_left = parameterB;
regression(2, right_turn_down[0] - 3, right_turn_down[0] + 2);//右线
trend_of_right = parameterB;
}
}
twolines_trend = juge_if_same_fuhao(trend_of_left, trend_of_right);
//大弯道拐点
if (twolines_trend == 0)
{
if (left_turn_down[0] != 0 && right_turn_down[0] == 0)//左下拐点存在而右下拐点不存在
{
curve_guai[0] = left_turn_down[0];
curve_guai[1] = centerline[left_turn_down[0]];
}
else if (right_turn_down[0] != 0 && left_turn_down[0] == 0) //右下拐点存在而左下拐点不存在
{
curve_guai[0] = right_turn_down[0];
curve_guai[1] = centerline[right_turn_down[0]];
}
else if (left_turn_down[0] != 0 && left_turn_down[0] != 0) //左右拐点均存在
{
curve_guai[0] = (right_turn_down[0] + left_turn_down[0]) / 2;
curve_guai[1] = centerline[(right_turn_down[0] + left_turn_down[0]) / 2];
}
//if (curve_guai[0] != 0) SetText("弯道拐点： " + curve_guai[0] + " " + curve_guai[1]);
}
SetText("trend_of_left: " + trend_of_left + " trend_of_right: " + trend_of_right);
//经过限制后的下拐点 首先是存在拐点，并且两条线趋势相异，或上相异的线丢失了
if ((left_turn_down[0] != 0 && twolines_trend == 1) || (lostright_times >= 25 && left_turn_down[0] != 0))
{
SetText("新的左下拐点 ");
SetText(" " + left_turn_down[0] + " " + left_turn_down[1]);
findleftdownguai = 1; //表示找到左下拐点了
}
else findleftdownguai = 0;
if ((right_turn_down[0] != 0 && twolines_trend == 1) || (lostleft_times >= 25 && right_turn_down[0] != 0))
{
SetText("新的右下拐点 ");
SetText(" " + right_turn_down[0] + " " + right_turn_down[1]);
findrightdownguai = 1;//表示找到右下拐点了
}
else findrightdownguai = 0;
/*************找到左下或右下拐点后，拟合并预测中线，然后再顺着预测后的中线找**************/
if (findrightdownguai == 1 || findleftdownguai == 1)
{
SetText("拟合预测中线 ");
int start = 0;
int end = 0;
if (findrightdownguai == 1 && findleftdownguai == 0)//左斜入十字，仅有右下拐点，取右下拐点下的中线行
{
SetText("左斜入十字 ");
start = right_turn_down[0] - 12;
end = right_turn_down[0] - 1;
if (start <= 0) start = 0;
if (end <= 1) end = 1;
regression(0, start, end);
parameterB = parameterB + 0.3f;
}
else if (findrightdownguai == 0 && findleftdownguai == 1) //右斜入十字，仅有左下拐点，取左下拐点下的中线行
{
SetText("右斜入十字 ");
start = left_turn_down[0] - 12;
end = left_turn_down[0] - 1;
if (start <= 0) start = 0;
if (end <= 1) end = 1;
regression(0, start, end);
parameterB = parameterB - 0.3f;
}
else if (findrightdownguai == 1 && findleftdownguai == 1) //正入十字：用两个下拐点中最小行下的中线行，拟合出k，b，进而拟合出预测中线
{
SetText("正入十字 ");
start = My_Min(left_turn_down[0], right_turn_down[0]) - 12;
end = My_Min(left_turn_down[0], right_turn_down[0]) - 1;
if (start <= 0) start = 0;
if (end <= 1) end = 1;
regression(0, start, end); //start是0，end是左下拐点和右下拐点的平均行数
}
//这样得出了拟合出的k和b
//预测新的中线值
//这时候forecast_centerline
SetText("拟合出了中线 ");
for (i = 0; i < 70; i++)
{
forecast_centerline[i] = overflow((int)(parameterB * i + parameterA));
if (forecast_centerline[i] >= 185 || forecast_centerline[i] <= 0)
{
break_hangshu = i;
break;
}
R_Start[i] = forecast_centerline[i];
//SetText(forecast_centerline[i]);
}
second_scan_flag = 1;
}
/********************第二次扫线（顺着预测出来的点往两边扫）********************/
//这时就不需要计数times和flag_s了
if (second_scan_flag == 1)
{
SetText("顺着预测出来的点往两边扫 ");
for (j = 0; j < 70; j++)
{
leftfindflag[j] = 0;
rightfindflag[j] = 0;
for (i = (byte)forecast_centerline[j]; i >= 0 && i < 184; i++)
{
if (Pixels[j, i + 1] == 0 && Pixels[j, i] == 0)
{
leftfindflag[j] = 1;
lefetline[j] = (byte)(i + 1);
break;
}
}
for (i = (byte)forecast_centerline[j]; i <= 185 && i > 1; i--)
{
if (Pixels[j, i - 1] == 0 && Pixels[j, i] == 0)
{
rightfindflag[j] = 1;
rightline[j] = (byte)(i - 1);
break;
}
}
if (leftfindflag[j] == 0 && rightfindflag[j] != 0) lefetline[j] = 185;
if (rightfindflag[j] == 0 && leftfindflag[j] != 0) rightline[j] = 0;
centerline[j] = (byte)((lefetline[j] + rightline[j]) / 2);
if ((Pixels[j, centerline[j]]) == 0 && (Pixels[j + 1, centerline[j]]) == 0)
{
break_hangshu = j;
SetText("补完线的breakhangshu:" + break_hangshu);
if (break_hangshu >= 20) //防止在一开始就break
{
break;
}
break;
}
}
//把break行以上的数据清除（也可以选择不清）
for (j = break_hangshu; j < 70; j++)
{
lefetline[j] = 185;
rightline[j] = 0;
}
//由于第一次扫线有误，所以上拐点位置大概率是错误的，需要重新检测
//首先清零(也可以选择不清)
//left_turn_up[0] = 0;
//left_turn_up[1] = 0;
//right_turn_up[0] = 0;
//right_turn_up[1] = 0;
/*************扫完第二次线，再找上拐点****************************/
/***找左上拐点***********/
for (j = 2; j <= 55; j++)
{
//左上拐点
if (((j > (byte)left_turn_down[0]) && lefetline[j] - lefetline[j - 1] <= -3 && My_Abs(lefetline[j + 1] - lefetline[j]) <= 2 && My_Abs(lefetline[j + 2] - lefetline[j + 1]) <= 2)
|| ((j > (byte)left_turn_down[0]) && lefetline[j - 2] - lefetline[j] >= 50 && lefetline[j - 1] - lefetline[j] >= 50 && My_Abs(lefetline[j + 1] - lefetline[j]) <= 3)
&& leftfindflag[j] == 1 && leftfindflag[j + 1] == 1 && leftfindflag[j + 2] == 1)
{
left_turn_up[0] = j + 1;//数组里面没有第0行
left_turn_up[1] = lefetline[j];
//获得的上坐标先确定一下是不是比下坐标小，如果小则说明提前断掉，此时的“上拐点”为假.
//如果比下坐标大则此时的“上拐点”为真.
if (left_turn_up[0] <= left_turn_down[0])
{
;
}
else break;
}
}
/***找右上拐点***********/
for (j = 2; j <= 55; j++)
{
if (((j > (byte)right_turn_down[0]) && (rightline[j] - rightline[j - 1]) >= 3 && My_Abs(rightline[j + 1] - rightline[j]) <= 2 && My_Abs(rightline[j + 2] - rightline[j + 1]) <= 2)
|| ((j > (byte)right_turn_down[0]) && rightline[j - 2] - rightline[j] <= -50 && rightline[j - 1] - rightline[j] <= -50 && My_Abs(rightline[j + 1] - rightline[j]) <= 3)
&& (rightfindflag[j] == 1 && rightfindflag[j + 1] == 1 && rightfindflag[j + 2] == 1))
{
right_turn_up[0] = j + 1;
right_turn_up[1] = rightline[j];
if (right_turn_up[0] <= right_turn_down[0])
{
;
}
else break;
}
}
if (right_turn_up[0] > right_turn_down[0] && right_turn_up[0] != 0)
{
SetText("新的右上拐点 ");
SetText(" " + right_turn_up[0] + " " + right_turn_up[1]);
findrightupguai = 1;//表示找到右上拐点了
}
if (left_turn_up[0] > left_turn_down[0] && left_turn_up[0] != 0)
{
SetText("新的左上拐点 ");
SetText(" " + left_turn_up[0] + " " + left_turn_up[1]);
findleftupguai = 1;//表示找到左上拐点了
}
}
/*********开始补线(找到左下拐点和左上拐点 或 找到右下拐点和右上拐点)*********/
if (flag_small_S == 0 && (findleftupguai == 1 && findleftdownguai == 1) || (findrightupguai == 1 && findrightdownguai == 1) || (findrightupguai == 1 && findrightdownguai == 0) || (findleftupguai == 1 && findleftdownguai == 0))
{
flag_shizi = 1;
if (findleftupguai == 1 && findleftdownguai == 1) //找到左下拐点和左上拐点,拟合所需的点是下拐点下面三个点和上拐点上面三个点
{
int start1 = left_turn_down[0] - 3;
if (start1 <= 0) start1 = 0;
int end1 = left_turn_down[0] - 1;
int start2 = left_turn_up[0];
if (start2 <= 0) start2 = 0;
int end2 = left_turn_up[0] + 3;
advanced_regression(1, start1, end1, start2, end2);
/***********需要补的是上下两个拐点之间的点*******************************/
for (j = (byte)end1; j < (byte)start2; j++)
{
byte jicun = lefetline[j];
lefetline[j] = overflow((int)(parameterB * j + parameterA));
//同时加上限制条件，如果左线在中线的右边,则保留原来的坐标
if (lefetline[j] < (rightline[j] + lefetline[j]) / 2) lefetline[j] = jicun;
}
}
else if (findleftupguai == 1 && findleftdownguai == 0)
{
int start1 = left_turn_up[0];
if (start1 <= 0) start1 = 0;
int end1 = left_turn_up[0] + 5;
regression(1, start1, end1);
/***********需要补的是上下两个拐点之间的点*******************************/
for (j = 0; j < (byte)start1; j++)
{
byte jicun = lefetline[j];
lefetline[j] = overflow((int)(parameterB * j + parameterA));
if (lefetline[j] < (rightline[j] + lefetline[j]) / 2) lefetline[j] = jicun;
}
}
//并列关系
if (findrightdownguai == 1 && findrightupguai == 1) //找到左下拐点和左上拐点,拟合所需的点是下拐点下面三个点和上拐点上面三个点
{
int start1 = right_turn_down[0] - 3;
if (start1 <= 0) start1 = 0;
int end1 = right_turn_down[0] - 1;
int start2 = right_turn_up[0];
if (start2 <= 0) start2 = 0;
int end2 = right_turn_up[0] + 3;
advanced_regression(2, start1, end1, start2, end2);
/***********需要补的是上下两个拐点之间的点*******************************/
for (j = (byte)end1; j < (byte)start2; j++)
{
byte jicun = rightline[j];
rightline[j] = overflow((int)(parameterB * j + parameterA));
if (rightline[j] > (rightline[j] + lefetline[j]) / 2) rightline[j] = jicun;
}
}
else if (findrightdownguai == 0 && findrightupguai == 1)
{
int start1 = right_turn_up[0];
if (start1 <= 0) start1 = 0;
int end1 = right_turn_up[0] + 5;
regression(2, start1, end1);
/***********需要补的是上下两个拐点之间的点*******************************/
for (j = 0; j < (byte)start1; j++)
{
byte jicun = rightline[j];
rightline[j] = overflow((int)(parameterB * j + parameterA));
if (rightline[j] > (rightline[j] + lefetline[j]) / 2) rightline[j] = jicun;
}
}
}
//存入显示数组中
for (j = 0; j < 70; j++)
{
centerline[j] = (byte)((lefetline[j] + rightline[j]) / 2);
LCenter[j] = centerline[j];
L_black[j] = lefetline[j];
R_black[j] = rightline[j];
}
}
/********【b2】入十字了，用上面两个点拉下来************/
else if ((times > 6 && (l_start > 15 && r_start > 15 && My_Abs(l_start - r_start) <= 8)))
{
//因为入十字了有许多空白行，会导致中线不准，沿中线往两边扫描得到的拐点也会不准，所以需要重新扫线，此时选择固定行扫线，且times和flag_s不再计数
for (j = 0; j < 70; j++)
{
leftfindflag[j] = 0;
rightfindflag[j] = 0;
for (i = 93; i >= 0 && i < 184; i++)
{
if (Pixels[j, i + 1] == 0 && Pixels[j, i] == 0)
{
leftfindflag[j] = 1;
lefetline[j] = (byte)(i + 1);
break;
}
}
for (i = 93; i <= 185 && i > 1; i--)
{
if (Pixels[j, i - 1] == 0 && Pixels[j, i] == 0)
{
rightfindflag[j] = 1;
rightline[j] = (byte)(i - 1);
break;
}
}
if (leftfindflag[j] == 0 && rightfindflag[j] != 0) lefetline[j] = 185;
if (rightfindflag[j] == 0 && leftfindflag[j] != 0) rightline[j] = 0;
centerline[j] = (byte)((lefetline[j] + rightline[j]) / 2);
if ((Pixels[j, centerline[j]]) == 0 && (Pixels[j + 1, centerline[j]]) == 0)
{
break_hangshu = j;
if (break_hangshu >= 20) //防止在一开始就break
{
break;
}
}
}
//breakhang以上的清数据
for (j = break_hangshu; j < 70; j++)
{
lefetline[j] = 185;
rightline[j] = 0;
}
/*************扫完第二次线，再找上拐点****************************/
/***找左上拐点***********/
for (j = 2; j <= 55; j++)
{
//左上拐点
if (((j > (byte)left_turn_down[0]) && lefetline[j] - lefetline[j - 1] <= -3 && My_Abs(lefetline[j + 1] - lefetline[j]) <= 2 && My_Abs(lefetline[j + 2] - lefetline[j + 1]) <= 2)
|| ((j > (byte)left_turn_down[0]) && lefetline[j - 2] - lefetline[j] >= 50 && lefetline[j - 1] - lefetline[j] >= 50 && My_Abs(lefetline[j + 1] - lefetline[j]) <= 3)
&& leftfindflag[j] == 1 && leftfindflag[j + 1] == 1 && leftfindflag[j + 2] == 1)
{
left_turn_up[0] = j + 1;//数组里面没有第0行
left_turn_up[1] = lefetline[j];
if (left_turn_up[0] <= left_turn_down[0])
{
;
}
else break;
}
}
/***找右上拐点***********/
for (j = 2; j <= 55; j++)
{
//右上拐点
if (((j > (byte)right_turn_down[0]) && (rightline[j] - rightline[j - 1]) >= 3 && My_Abs(rightline[j + 1] - rightline[j]) <= 2 && My_Abs(rightline[j + 2] - rightline[j + 1]) <= 2)
|| ((j > (byte)right_turn_down[0]) && rightline[j - 2] - rightline[j] <= -50 && rightline[j - 1] - rightline[j] <= -50 && My_Abs(rightline[j + 1] - rightline[j]) <= 3)
&& (rightfindflag[j] == 1 && rightfindflag[j + 1] == 1 && rightfindflag[j + 2] == 1))
{
right_turn_up[0] = j + 1;
right_turn_up[1] = rightline[j];
if (right_turn_up[0] <= right_turn_down[0])
{
;
}
else break;
}
}
if (right_turn_up[0] > right_turn_down[0] && right_turn_up[0] != 0)
{
SetText("新的右上拐点 ");
SetText(" " + right_turn_up[0] + " " + right_turn_up[1]);
findrightupguai = 1;//表示找到右上拐点了
}
if (left_turn_up[0] > left_turn_down[0] && left_turn_up[0] != 0)
{
SetText("新的左上拐点 ");
SetText(" " + left_turn_up[0] + " " + left_turn_up[1]);
findleftupguai = 1;//表示找到左上拐点了
}
SetText("拉线 ");
//if (flag_small_S == 0)
//{
if (right_turn_up[0] != 0)
{
flag_shizi = 1;
SetText("拉右线 ");
int start1 = right_turn_up[0];
if (start1 <= 0) start1 = 0;
int end1 = right_turn_up[0] + 5;
regression(2, start1, end1);
int jicun1;
/***********需要补的是上下两个拐点之间的点*******************************/
for (j = 0; j < (byte)start1; j++)
{
jicun1 = (int)(parameterB * j + parameterA);
if (jicun1 >= 185) jicun1 = 185;
else if (jicun1 <= 0) jicun1 = 0;
//if (rightline[j] > (rightline[j] + lefetline[j]) / 2)
rightline[j] = (byte)jicun1;
}
SetText("拉右线结束");
}
if (left_turn_up[0] != 0)
{
flag_shizi = 1;
int start1 = left_turn_up[0];
if (start1 <= 0) start1 = 0;
int end1 = left_turn_up[0] + 5;
regression(1, start1, end1);
int jicun2;
/***********需要补的是上下两个拐点之间的点*******************************/
for (j = 0; j < (byte)start1; j++)
{
//if (lefetline[j] < (rightline[j] + lefetline[j]) / 2)
jicun2 = (int)(parameterB * j + parameterA);
if (jicun2 >= 185) jicun2 = 185;
else if (jicun2 <= 0) jicun2 = 0;
lefetline[j] = (byte)jicun2;
}
}
//}
for (j = 0; j < 70; j++)
{
centerline[j] = (byte)((lefetline[j] + rightline[j]) / 2);
LCenter[j] = centerline[j];
L_black[j] = lefetline[j];
R_black[j] = rightline[j];
}
}
}
SetText("breakhangshu" + break_hangshu);
SetText("last_break_hangshu" + last_break_hangshu);
十字补线后，将十字就相当于直道。（十字不涉及道路判断）
6、中线断线补线（补breakhang之后的线，防止breakhang过低，取PID得点取到虚空处）
bu_breakhang(2, 15, break_hangshu);
void bu_breakhang(int c1, int c2, byte j)
{
int k = centerline[c2] - centerline[c1];
//SetText("kkkkkk" + k);
if (k > 10) //入左弯
{
for (byte i = j; i < 70; i++)
{
centerline[i] = 185;
LCenter[i] = centerline[i];
//SetText(" da");
}
}
else if (k < -10) //入右弯
{
for (byte i = j; i < 70; i++)
{
centerline[i] = 0;
}
}
else// 直到置0
{
for (byte i = j; i < 70; i++)
{
centerline[i] = 93;
}
}
}
7、计算40行以内的中线的偏差和
for (j = 1; j < 40; j++)
{
center_delta = centerline[j + 1] - centerline[j]; //中线的偏差
sumofcenter += center_delta;
}
//然后结果取个绝对值
sumofcenter = My_Abs(sumofcenter);
8、计算40行以内的中线的最大值和最小值并且计算出两者之差
注意：每帧图像的max和min预先置为93（即图像最中间）
//计算40行以内的中线的最大值和最小值并且计算出两者之差
for (j = 0; j < 40; j++)
{
if (centerline[j] >= max) max = centerline[j];
if (centerline[j] <= min) min = centerline[j];
}
center_max_min_delta = max - min;
9、对初步扫出的线进行分析连续性（对中线）
juge_lineContinuity(10, 60, 3, -3, CENTER);
SetText("中线long_turn_flag:" + long_turn_flag);
10、保存三线值，并将这一帧的第6行中线值保存为old，用到下一帧。
for (j = 0; j < 70; j++)
{
R_Start[j] = forecast_centerline[j];
LCenter[j] = centerline[j];
L_black[j] = lefetline[j];
R_black[j] = rightline[j];
}
old = centerline[5];
总结
至此，初步的扫线和基本特征已经完成，下面就是利用我们所提取的特征进行判断道路类型了，在环岛道路时还需要重新扫线。