【智能车Code review】——拐点的寻找
CROSSROAD：十字拐点
ROUNDISLAND：环岛拐点
rightfindflag[i]:表示第i行的右线是否被找到，1为找到，0为没有找到
flag_find_huan_rightdown_point :找到环岛右下拐点标志
flag_find_huan_rightmiddle_point:找到环岛右中拐点标志
找拐点的思路：观察一段数据中数据点的走向趋势，并且保证这段数据是准确的。在摄像头畸变比较严重的情况下这种方法不再适用
关于函数的调用请见：https://blog.csdn.net/qq_42604176/article/details/105336462
【1】找右下拐点函数
//【Function5】找右下拐点函数
//输入： start点 end点 所要判断的下拐点的类型（环岛还是十字？）
void find_rightdown_point(byte start_point, byte end_point, byte RoadName)
{
byte j;
if (RoadName == CROSSROAD)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (My_Abs(rightline[j - 1] - rightline[j - 2]) <= 3 && My_Abs(rightline[j] - rightline[j - 1]) <= 3 && (rightline[j + 1] - rightline[j] <= -2)
&& rightfindflag[j - 2] == 1 && rightfindflag[j - 1] == 1 && rightfindflag[j] == 1)
{
right_turn_down[0] = j + 1;//数组里面没有第0行
right_turn_down[1] = rightline[j];
break;
}
}
}
else if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (r_start <= 34 && My_Abs(rightline[j - 1] - rightline[j - 2]) <= 3 && My_Abs(rightline[j] - rightline[j - 1]) <= 3 && (rightline[j + 1] - rightline[j] <= -2)
&& rightfindflag[j - 2] == 1 && rightfindflag[j - 1] == 1 && rightfindflag[j] == 1)
{
right_turn_down[0] = j + 1;//数组里面没有第0行
right_turn_down[1] = rightline[j];
flag_find_huan_rightdown_point = 1;
break;
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
if (right_turn_down[0] != 0 && huandao_memory != 4) flag_find_huan_rightdown_point = 1;
}
else
{
}
}
【2】找左下拐点函数
//【Function6】找左下拐点函数
//输入： start点 end点 所要判断的下拐点的类型（环岛还是十字？）
void find_leftdown_point(byte start_point, byte end_point, byte RoadName)
{
byte j;
if (RoadName == CROSSROAD)
{
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if (My_Abs(lefetline[j - 1] - lefetline[j - 2]) <= 3 && My_Abs(lefetline[j] - lefetline[j - 1]) <= 3 && (lefetline[j + 1] - lefetline[j] >= 3)
&& leftfindflag[j - 2] == 1 && leftfindflag[j - 1] == 1 && leftfindflag[j] == 1)
{
left_turn_down[0] = j + 1;//数组里面没有第0行
left_turn_down[1] = lefetline[j];
break;
}
}
}
else if (RoadName == ROUNDISLAND)
{
//setText用户自定义("l_start"+ l_start);
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if (l_start <= 34 && My_Abs(lefetline[j - 1] - lefetline[j - 2]) <= 3 && My_Abs(lefetline[j] - lefetline[j - 1]) <= 3 && (lefetline[j + 1] - lefetline[j] >= 3)
&& leftfindflag[j - 2] == 1 && leftfindflag[j - 1] == 1 && leftfindflag[j] == 1)
{
left_turn_down[0] = j + 1;//数组里面没有第0行
left_turn_down[1] = lefetline[j];
flag_find_huan_leftdown_point = 1;
break;
}
}
if (left_turn_down[0] != 0 && huandao_memory != 4) flag_find_huan_leftdown_point = 1;
}
else
{
}
}
【3】找右中拐点函数
//【Function7】找右中拐点函数
//输入： start点 end点
void find_rightmiddle_point(byte start_point, byte end_point)
{
byte i;
for (i = start_point; i <= end_point; i++)
{
//找you拐点
if (i >= 8 && (rightline[i - 5] - rightline[i - 1]) < 0 && (rightline[i - 4] - rightline[i - 1]) < 0 && (rightline[i - 3] - rightline[i - 1]) < 0
&& (rightline[i - 5] - rightline[i - 4]) <= 0 && (rightline[i - 4] - rightline[i - 3]) <= 0 && (rightline[i - 3] - rightline[i - 2]) <= 0
&& (rightline[i + 4] - rightline[i - 1]) <= 0 && (rightline[i + 3] - rightline[i - 1]) <= 0 && (rightline[i + 2] - rightline[i - 1]) <= 0
&& (rightline[i + 4] - rightline[i + 3]) <= 0 && (rightline[i + 3] - rightline[i + 2]) <= 0
)
{
right_turn_middle[0] = (byte)(i - 1);
right_turn_middle[1] = rightline[i - 1];
flag_find_huan_rightmiddle_point = 1;
break;
}
}
}
【4】找左中拐点函数
//【Function8】找左中拐点函数
//输入： start点 end点
void find_leftmiddle_point(byte start_point, byte end_point)
{
byte i;
for (i = start_point; i <= end_point; i++)
{
//找左拐点
if (i >= 8 && (lefetline[i - 5] - lefetline[i - 1]) > 0 && (lefetline[i - 4] - lefetline[i - 1]) > 0 && (lefetline[i - 3] - lefetline[i - 1]) > 0
&& (lefetline[i - 5] - lefetline[i - 4]) >= 0 && (lefetline[i - 4] - lefetline[i - 3]) >= 0 && (lefetline[i - 3] - lefetline[i - 2]) >= 0
&& (lefetline[i + 4] - lefetline[i - 1]) >= 0 && (lefetline[i + 3] - lefetline[i - 1]) >= 0 && (lefetline[i + 2] - lefetline[i - 1]) >= 0
//&& (lefetline[i + 4] - lefetline[i + 3]) > 0 && (lefetline[i + 3] - lefetline[i + 2]) >= 0
)
{
left_turn_middle[0] = (byte)(i - 1);
left_turn_middle[1] = lefetline[i - 1];
flag_find_huan_leftmiddle_point = 1;
break;
}
}
}
【5】找右上拐点函数
//【Function9】找右上拐点函数
//输入： start点 end点 所要判断的下拐点的类型（环岛还是十字？）
void find_rightup_point(byte start_point, byte end_point, byte RoadName)
{
byte j;
if (RoadName == CROSSROAD)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
if (flag_s >= 47) break;
//右上拐点
if ((rightline[j - 1] - rightline[j - 2]) >= 3 && My_Abs(rightline[j] - rightline[j - 1]) <= 2 && My_Abs(rightline[j + 1] - rightline[j]) <= 2
&& rightfindflag[j - 1] == 1 && rightfindflag[j] == 1 && rightfindflag[j + 1] == 1)
{
right_turn_up[0] = j + 1;
right_turn_up[1] = rightline[j];
break;
}
}
}
else if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((rightline[j - 1] - rightline[j - 2]) >= 4 && My_Abs(rightline[j] - rightline[j - 1]) <= 3 && My_Abs(rightline[j + 1] - rightline[j]) <= 3
&& rightfindflag[j - 1] == 1 && rightfindflag[j] == 1 && rightfindflag[j + 1] == 1)
{
right_turn_up[0] = j + 1;
right_turn_up[1] = rightline[j];
flag_find_huan_rightup_point = 1;
if (((right_turn_middle[0] > 0 && right_turn_middle[0] <= 50 && right_turn_up[1] <= right_turn_middle[1]) || right_turn_up[1] <= 15) && huandao_memory == 4)
{
}
else
{
break;
}
break;
}
}
}
else
{
}
}
【6】找左上拐点函数
//【Function10】找左上拐点函数
//输入： start点 end点 所要判断的下拐点的类型（环岛还是十字？）
void find_leftup_point(byte start_point, byte end_point, byte RoadName)
{
byte j;
if (RoadName == CROSSROAD)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
if (flag_s >= 47) break;
//左上拐点
//lefetline[j - 2] - lefetline[j - 3] <= -4 &&
if (lefetline[j - 1] - lefetline[j - 2] <= -3 && My_Abs(lefetline[j] - lefetline[j - 1]) <= 2 && My_Abs(lefetline[j + 1] - lefetline[j]) <= 2
&& leftfindflag[j - 1] == 1 && leftfindflag[j] == 1 && leftfindflag[j + 1] == 1)
{
left_turn_up[0] = j + 1;//数组里面没有第0行
left_turn_up[1] = lefetline[j];
if (left_turn_up[1] < 120)
{
break;
}
else
{
}
}
}
}
else if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//左上拐点
if (lefetline[j - 1] - lefetline[j - 2] <= -4 && My_Abs(lefetline[j] - lefetline[j - 1]) <= 3 && My_Abs(lefetline[j + 1] - lefetline[j]) <= 3
&& leftfindflag[j - 1] == 1 && leftfindflag[j] == 1 && leftfindflag[j + 1] == 1)
{
left_turn_up[0] = j + 1;//数组里面没有第0行
left_turn_up[1] = lefetline[j];
flag_find_huan_leftup_point = 1;
if (((left_turn_middle[0] > 0 && left_turn_middle[0] <= 50 && left_turn_up[1] >= left_turn_middle[1])|| left_turn_up[1]>=170) && huandao_memory==4)
{
}
else
{
break;
}
}
}
}
else
{
}
}