智能车图像处理9-左环岛状态机与补线
前言
这篇文章主要讲述智能车竞赛中如何判断左环岛并且进行补线操作。
一、函数主体
//左环岛主体开始
void juge_left_succesive_and_if_k_limitforleft()
{
if (huandaotype!=0&&turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && rukuflag == 0 && (threemode == 0 || threemode == 6 || threemode == 12) && firstku == 0&&countfirstku==0)
{
if (truedoubleleft != 0 || huandao_memoryforleft != 0)
{
if (huandao_memory == 0)
{ //可能是环岛的地带 -0.17
leftdown15white = 1;
int i;
for (i = 0; i < 15; i++)
{
if (L_black[i] != 186)
{
leftdown15white = 0;
break;
}
}
cakkuandu();
flag_find_huan_leftdown_point = 0;
flag_find_huan_rightdown_point = 0;
flag_find_huan_rightmiddle_point = 0;
flag_find_huan_leftmiddle_point = 0;
flag_find_huan_rightup_point = 0;
flag_find_huan_leftup_point = 0;
findhuan_rightdown_point(5, 32, ROUNDISLAND); ///环岛找拐点开始
if (huandaotype == 6&& (huandao_memoryforleft==6|| huandao_memoryforleft==7))
{
flag_find_huan_rightdown_point = 0;
findhuan100_rightdown_point(5, 32, ROUNDISLAND);
} ///环岛找拐点开始}
if (huandao_memoryforleft !=4&&huandaotype!=3&& huandao_memoryforleft != 3) //左下 非70环岛
{ findhuan_leftdown_point(5, 28, ROUNDISLAND); }
if ((huandao_memoryforleft == 4|| huandao_memoryforleft==3) && huandaotype != 3)
{ findhuan_leftdown_point(5, 18, ROUNDISLAND); } //左下
if (huandao_memoryforleft != 4 && huandaotype == 3&&huandao_memoryforleft != 3 ) //左下 三环到
{
findhuan_leftdown_point(5, 28, ROUNDISLAND);
}
if ((huandao_memoryforleft == 4|| huandao_memoryforleft == 3) && huandaotype == 3)
{
findhuan_leftdown_point(5, 20, ROUNDISLAND);
} //左下
if (flag_find_huan_rightdown_point == 1 && (right_turn_down[0] + 1) < 45)//右中 下找到
{ findhuan_rightmiddle_point(right_turn_down[0] + 3, 45); }
if (flag_find_huan_rightdown_point == 0)
{ findhuan_rightmiddle_point(3, 45); }
if (flag_find_huan_leftdown_point == 1 && (left_turn_down[0] + 6) < 45&&huandaotype!=3) //左中 下找到 //四的时候中拐点拉近
{
if (huandao_memoryforleft == 4 && left_turn_down[0] + 6 <= 30)
{ findhuan_leftmiddle_point(left_turn_down[0] + 6, 45); }
if(huandao_memoryforleft!=4) findhuan_leftmiddle_point(left_turn_down[0] + 6, 45);
}
if (flag_find_huan_leftdown_point == 0&&huandaotype!=3)
{
if (huandao_memoryforleft == 4)
{ findhuan_leftmiddle_point(3, 25); }
if (huandao_memoryforleft != 4)
{ findhuan_leftmiddle_point(3, 45); }
} //左中
if (flag_find_huan_leftdown_point == 1 && (left_turn_down[0] ++) < 45 && huandaotype == 3) //左中 下找到
{
if (huandao_memoryforleft != 3)
{
findhuan_leftmiddle_point(left_turn_down[0] + 6, 45);
}
if (huandao_memoryforleft == 3)
{
findhuan_leftmiddle_point(left_turn_down[0] + 6, 45);
}
}
if (flag_find_huan_leftdown_point == 0 && huandaotype == 3)
{
if (huandao_memoryforleft != 3&& huandao_memoryforleft != 4)
{
findhuan_leftmiddle_point( 3, 45);
}
if (huandao_memoryforleft == 3 || huandao_memoryforleft == 4)
{
if (leftdown15white == 1)
{ findhuan_leftmiddle_point(3, 38); }
if (leftdown15white == 0)
{ findhuan_leftmiddle_point(3, 38); }
}
} //左中
if (flag_find_huan_rightmiddle_point == 1 && (right_turn_middle[0] + 2) < 50)
{ findhuan_rightup_point(right_turn_middle[0] + 2, 50, ROUNDISLAND); }
if (flag_find_huan_rightmiddle_point == 0)
{ findhuan_rightup_point(25, 50, ROUNDISLAND); }
if (flag_find_huan_leftmiddle_point == 1 && (left_turn_middle[0] + 2) < 50)
{ findhuan_leftup_point(left_turn_middle[0] + 2, 50, ROUNDISLAND); }
if (flag_find_huan_leftmiddle_point == 0)
{ findhuan_leftup_point(25, 50, ROUNDISLAND); } //环岛找拐点结束
if (huandao_memoryforleft >= 5) { flag_find_huan_leftmiddle_point = 0; findhuan_leftmiddle_point(3, 40); }
if ((huandao_memoryforleft == 7|| huandao_memoryforleft == 6) ) { flag_find_huan_rightout_point = 0; findhuan_rightout_point(5, 35, ROUNDISLAND); }
if (break_hangshu >= 5) lcenter_5 = LCenter[break_hangshu - 4];
if (break_hangshu <= 5) lcenter_5 = 0;
int begin = 0;
for (i = 3; i < 50; i++)
{
if (L_black[i] == 186 && L_black[i + 1] == 186 && L_black[i + 2] != 186)
{
begin = i + 2;
break;
}
}
if (huandao_memoryforleft == 7 && begin > 3&&huandaotype!=6) { flag_find_huan_leftup_point = 0; findhuan_leftup_point77(3, begin+5, ROUNDISLAND); }
regression(2, break_hangshu - 14, break_hangshu - 12);
if (huandao_memoryforleft <= 8)
{
/*******行驶到环岛出口或者在行驶到环岛出口前********/
if (huandao_memoryforleft <= 3)
{
if (huandao_memoryforleft == 1 && left_turn_down[0] <= 22 && left_turn_down[0] > 0 && break_hangshu >= 35 && flag_find_huan_leftdown_point == 1)
{
huandao_memoryforleft = 2;
huandao_flag_R_L = 1;
}
else if (huandao_memoryforleft == 2)
{
if ((L_black[5] >= 160)
)
{
// SetText("行驶到环岛出口（没进环岛呢）");
huandao_memoryforleft = 3;
huandao_flag_R_L = 1;
}
}
else if (huandao_memoryforleft == 0 && left_turn_down[0] > 5 && left_turn_down[0] <= 40 &&
flag_find_huan_leftdown_point == 1 &&
(calkuan[14] > 105 || calkuan[15] > 105 || calkuan[16] > 105 || calkuan[17] > 105 || calkuan[18] > 105
|| calkuan[19] > 105 || calkuan[20] > 105 || calkuan[21] > 105 || calkuan[22] > 105)
&& truepianfangflagforleft == 1 && continueright == 1 && huandao_memory == 0
&& break_hangshu >= 37)
{
huandao_memoryforleft = 1;
crossrecord = 0;
crossderecord = 0;
cross31 = 0;
huandao_flag_R_L = 1;
//SetText("行驶到环岛出口前（没进环岛呢）(远)");
}
//快一些进入
else if ((huandao_memoryforleft == 0 || huandao_memoryforleft == 1) &&( left_turn_middle[0] > 5 && left_turn_middle[0] <= 35 &&
flag_find_huan_leftmiddle_point == 1 )&&
((L_black[14] > 182 && R_black[14] > 20) || (L_black[15] > 182 && R_black[15] > 20) || (L_black[16] > 182 && R_black[16] > 20) ||
(L_black[17] > 182 && R_black[17] > 20) || (L_black[18] > 182 && R_black[18] > 20) || (L_black[19] > 182 && R_black[19] > 20) ||
(L_black[20] > 182 && R_black[20] > 20) || (L_black[21] > 182 && R_black[21] > 20)||(calkuan[12]>155&& calkuan[13] > 155 && calkuan[14] > 155 &&
calkuan[15] > 155 && calkuan[16] > 155 && calkuan[17] > 155 ) ) && (truepianfangflagforleft == 1 && continueright == 1 && huandao_memory == 0)
&& break_hangshu >= 37&& left_turn_middle[0]<=break_hangshu-5)
{
huandao_memoryforleft = 3;
huandao_flag_R_L = 1;
crossrecord = 0;
crossderecord = 0;
cross31 = 0;
}
else if (huandao_memoryforleft == 3 && ((left_turn_middle[0] >= 0 && left_turn_middle[0] <= 22&& flag_find_huan_leftmiddle_point==1) || (flag_find_huan_leftup_point==1&&left_turn_up[0]<33)) && huandaotype!=3)
{
huandao_memoryforleft = 4;
huandao_flag_R_L = 1;
}
else if (huandao_memoryforleft == 3 && ((left_turn_middle[0] >= 0 && left_turn_middle[0] <= 22 && flag_find_huan_leftmiddle_point == 1) ||(flag_find_huan_leftup_point == 1 && left_turn_up[0]<=33) ) && huandaotype == 3)
{
huandao_memoryforleft = 4;
huandao_flag_R_L = 1;
}
}
else if (huandao_memoryforleft == 4 && break_hangshu <= 33 && R_black[10] > 23&&huandaotype!=6)
{
huandao_memoryforleft = 5;
huandao_flag_R_L = 1;
// SetText("这里进去的5");
// SetText("行驶到环岛入口（没进环岛呢）");
}
else if (huandao_memoryforleft == 4 && break_hangshu <= 37 && R_black[10] > 23 && huandaotype == 6)
{
huandao_memoryforleft = 5;
huandao_flag_R_L = 1;
// SetText("这里进去的5");
// SetText("行驶到环岛入口（没进环岛呢）");
}
else if (huandao_memoryforleft == 5 && break_hangshu <= 33 && R_black[10] > 20 && huandaotype != 6)
{
huandao_memoryforleft = 6;
huandao_flag_R_L = 1;
// SetText("这里进去的6");
}
else if (huandao_memoryforleft == 5 && break_hangshu <= 37 && R_black[10] > 20 && huandaotype == 6)
{
huandao_memoryforleft = 6;
huandao_flag_R_L = 1;
// SetText("这里进去的6");
}
else if (huandao_memoryforleft == 6 && right_turn_down[0] >= 1 && right_turn_down[0] <= 32 && flag_find_huan_rightdown_point == 1&&huandaotype!=6 )
{
huandao_memoryforleft = 7;
huandao_flag_R_L = 1;
//SetText("出环了");
// SetText("这里进去的");
}
else if (huandao_memoryforleft == 6 && right_turn_down[0] >= 1 && right_turn_down[0] <= 32 && flag_find_huan_rightdown_point == 1 && huandaotype == 6)
{
huandao_memoryforleft = 7;
huandao_flag_R_L = 1;
//SetText("出环了");
// SetText("这里进去的");
}
else if (huandao_memoryforleft == 7 && flag_find_huan_leftup_point == 1 && left_turn_up[1] < 180 && R_black[12] >= 15 && R_black[left_turn_up[0]] < 120
&& LCenter[10] > 80 && R_black[left_turn_up[0] + 4] > R_black[left_turn_up[0] + 2] && flag_find_huan_rightout_point == 0 && flag_find_huan_rightdown_point == 0)/**/
{
huandao_memoryforleft = 8;
huandao_flag_R_L = 1;
}
else if (huandao_memoryforleft == 8 && leftflag[13] == 1 && L_black[8] < 185 && L_black[9] < 185 && L_black[10] < 185
&& leftflag[14] == 1 && leftflag[15] == 1 && leftflag[16] == 1
&& L_black[13] < 185 && L_black[14] < 185 && L_black[15] < 185
&& L_black[16] < 185 && L_black[17] < 185 && (continueleft == 1 || (kuleftdownflag == 1 && kuleftupflag == 1) || (kurightupflag == 1 && kurightdownflag == 1)))
{
advanced_regression(2, 2, 4, 6, 8);
// SetText("8右线parameter斜率 " + parameterB);
if (parameterB < 2)
{
huandao_memoryforleft = 0;
huandao_flag_R_L = 0;
}
}
}
//SetText("flag_R_L " + huandao_flag_R_L);
R_mend_leftline_right_islandforleft((int)huandao_memoryforleft, (int)huandao_flag_R_L); //int
R_mend_rightline_right_islandforleft((int)huandao_memoryforleft, (int)huandao_flag_R_L);
//开启校验
if ((huandao_memoryforleft == 5 || huandao_memoryforleft == 6)&&huandaotype!=3)
{
advanced_regression(2, 6, 8, 10, 12);
if (parameterB == 0)
{
parameterA = 10;// 10 15
parameterB = 5;
R_blackfuzhi(parameterA, parameterB, 0, 60);
for (i = 0; i <= 60; i++)
{ L_black[i] = 186; }
}
//if (LCenter[20] < 120 && R_black[10] < 10)
//{
// parameterA = 10;// 10 15
// parameterB = 5;
// R_blackfuzhi(parameterA, parameterB, 0, 60);
// for (i = 0; i <= 60; i++)
// { L_black[i] = 186; }
//}
//if (LCenter[12] < 110 && R_black[10] < 10)
//{
// parameterA = 10;// 10 15
// parameterB = 5;
// R_blackfuzhi(parameterA, parameterB, 0, 60);
// for (i = 0; i <= 60; i++)
// { L_black[i] = 186; }
//}
}
last_memoryforleft = huandao_memoryforleft;
int j = 0;
if (huandao_memoryforleft <= 8 && huandao_memoryforleft != 0)
{
for (j = 0; j < 70; j++)
{
LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
}
}
}
}
}
}
//左环岛主体结束
二、辅助函数
//左环岛左线开始
int leftdown15white = 0;
void R_mend_leftline_right_islandforleft(int type, int flag_R_L)
{
int i = 0;
if (flag_R_L == 1)
{
if (type == 1 || type == 2)//状态机1和2操作从下拐点开始往上拉，拉到上拐点下五行 type右线赋值操作
{
int j = 0;
if (left_turn_down[0] >= 5 && left_turn_middle[0] >= 7 && left_turn_middle[0] > left_turn_down[0]&& flag_find_huan_leftmiddle_point==1&& flag_find_huan_leftdown_point==1)
{
advanced_regression(1, left_turn_down[0] - 2, left_turn_down[0], left_turn_middle[0] - 1, left_turn_middle[0]);
for (j = (int)left_turn_down[0]; j <= (int)left_turn_middle[0]; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
else
{
if (left_turn_down[0] >= 3)
{
advanced_regression(1, left_turn_down[0] - 3, left_turn_down[0]-2, left_turn_down[0] - 1, left_turn_down[0]);
for (j = 0; j <= (int)left_turn_middle[0]; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
}
}
else if (type == 3) //状态机三操作---仍旧拉底下的线
{
int j = 0;
if (flag_find_huan_leftmiddle_point == 1)
{
int delta = left_turn_middle[0] - 0;
//if (delta == 0) delta = 1;
float k = (left_turn_middle[1] - L_black[0]) * 1.0f / delta;
float b = L_black[0];
for (j = (int)0; j <= (int)left_turn_middle[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
else if (flag_find_huan_leftmiddle_point == 0 && flag_find_huan_leftdown_point == 1)
{
int delta = left_turn_down[0] - 0;
//if (delta == 0) delta = 1;
float k = (left_turn_down[1] - L_black[0]) * 1.0f / delta;
float b = L_black[0];
for (j = (int)0; j <= (int)left_turn_down[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
else
{
if (left_turn_middle[0] >= 5)
advanced_regression(1, left_turn_middle[0] - 5, left_turn_middle[0] -4, left_turn_middle[0] - 2, left_turn_middle[0]);
for (j = 0; j <= (int)left_turn_middle[0]; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
}
else if (type == 8)
{
int begin = 1;
if (huandaotype != 6)
{
for (i = 0; i < 50; i++)
{
if (L_black[i] == 186 && L_black[i] == 186 && L_black[i + 2] != 186 && L_black[i + 3] != 186)
{
begin = i + 2;
int j;
for (j = begin; j < 50; j++)
{
if (L_black[j] < 177)
{
begin = j;
break;
}
}
break;
}
}
}
if (huandaotype == 6)
{
for (i = 0; i < 50; i++)
{
if (Pixels[i][165] == 1 && Pixels[i+1][ 165] == 1 && Pixels[i+2][ 165] != 1 && Pixels[i+3][165] != 1)
{
begin = i + 2;
int j;
for (j = begin; j < 50; j++)
{
if (L_black[j] < 177)
{
begin = j;
break;
}
}
break;
}
}
}
if (begin != 1)
{
if (L_black[begin + 7] != 0)
{
advanced_regression(1, begin + 3, begin + 5, begin + 6, begin + 7);
monileftfuzhi(parameterA, parameterB, 0, 60);
if (monileft[2] > R_black[2] && monileft[6] > R_black[6] && monileft[10] > R_black[10] && monileft[14] > R_black[14])
{ leftlinefuzhi(parameterA, parameterB, 0, begin + 2); }
}
else
{
advanced_regression(1, begin + 1, begin + 2, begin + 4, begin + 5);
monileftfuzhi(parameterA, parameterB, 0, 60);
if (monileft[2] > R_black[2] && monileft[6] > R_black[6] && monileft[10] > R_black[10] && monileft[14] > R_black[14])
{ leftlinefuzhi(parameterA, parameterB, 0, begin + 2); }
}
}
else
{
advanced_regression(1, 28, 29, 30, 31);
monileftfuzhi(parameterA, parameterB, 0, 60);
if (monileft[2] > R_black[2] && monileft[6] > R_black[6] && monileft[10] > R_black[10] && monileft[14] > R_black[14])
{ leftlinefuzhi(parameterA, parameterB, 0, 40); }
}
}
else if (type == 4)
{
if (huandaotype != 3 || leftdown15white == 0)
{
if (left_turn_middle[0] >= 5 && flag_find_huan_leftmiddle_point == 1&& left_turn_middle[0]<30)
{
advanced_regression(1, left_turn_middle[0] - 5, left_turn_middle[0] - 3, left_turn_middle[0] - 2, left_turn_middle[0]);
leftlinefuzhi(parameterA, parameterB, 0, (int)(left_turn_middle[0] + 1));
}
if (left_turn_down[0] >= 5 && flag_find_huan_leftmiddle_point == 0 && flag_find_huan_leftdown_point == 1)
{
advanced_regression(1, left_turn_down[0] - 5, left_turn_down[0] - 3, left_turn_down[0] - 2, left_turn_down[0]);
leftlinefuzhi(parameterA, parameterB, 0, (int)(left_turn_down[0] + 1));
}
}
if ( leftdown15white == 1)
{
}
}
else if (type == 7) //状态7拉线
{
for (i = 0; i <= 60; i++)
{ L_black[i] = 186; }
}
}
}
//左环岛左线结束
//左环岛右线开始
void R_mend_rightline_right_islandforleft(int type, int flag_R_L) //int
{
if (flag_R_L == 1)
{
// SetText("TYPE:" + type);
if (type == 4) //状态机4的拉线 封住环岛前方赛道
{
int j;
int delta = left_turn_up[0] - 0;
if (delta == 0) delta = 1;
float k = (left_turn_up[1] - R_black[0]) * 1.0f / (delta * 1.0f);
// setText用户自定义("斜率" + k);
float b = R_black[0];
for (j = (int)0; j <= (int)left_turn_up[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
else if (type == 5) //重新扫线，从左边往右边扫 取消掉一半
{
}
else if (type == 6) //状态6正常处理
{
int j = 0;
int youguai_y = 0;
int youguai_x = 0;
int leftguai_y = 0;
int leftguai_x = 0;
/***找左下拐点***********/
for (j = 2; j <= 60; j++)
{
if (R_black[j - 1] - R_black[j - 2] >= 0 && R_black[j] - R_black[j - 1] <= 0 && (R_black[j + 1] - R_black[j] < 0)
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1)
{
leftguai_y = (int)(j - 1);
leftguai_x = R_black[j - 1];
break;
}
}
for (j = 8; j <= 60; j++)
{
//右连接处
if (L_black[j] != 0)
{
youguai_y = j;
youguai_x = L_black[j];
break;
}
}
if (leftguai_y != 0 && leftguai_x != 0 && youguai_y != 4 && youguai_x != 0)
{
if (youguai_y >= leftguai_y + 4)
{
jiansu_flag = 1;
}
else jiansu_flag = 0;
//开始拉线
int delta = (int)(youguai_y - leftguai_y);
if (delta == 0) delta = 1;
float k = (youguai_x - leftguai_x) * 1.0f / delta;
float b = leftguai_x - k * leftguai_y;
if (youguai_y >= leftguai_y)
{
for (j = (int)leftguai_y; j <= (int)youguai_y; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
}
}
else if (type == 7) //状态7拉线
{
//advanced_regression(2, 5, 7, 9, 11);
//SetText("parameterA7" + parameterA);
//SetText("parameterB7" + parameterB);
//parameterA = 0;// 10 15
//parameterB = 6;
//R_blackfuzhi(parameterA, parameterB, 0, 60);
int begin=3;
int i;
for (i = 1; i < 50; i++)
{
if (Pixels[i-1][170] == 1&&Pixels[i][ 170] == 1 && Pixels[i+1][ 170] != 1 && Pixels[i +2][170] != 1)
{
begin = i;
break;
}
}
if (begin == 3) { begin = 30; }
if (begin<=20) { begin = 22; }
// SetText("begin 77" + begin);
if (flag_find_huan_rightout_point == 1&& right_turn_out[1]>30)
{
R_black[begin] = 186;
R_black[begin - 1] = 150;
advanced_regression(2, right_turn_out[0] - 2, right_turn_out[0] , begin - 1, begin);
R_blackfuzhi(parameterA, parameterB, 0, 60);
}
// (flag_find_huan_rightout_point == 0)
else
{
R_black[begin] = 186;
R_black[begin - 1] = 150;
advanced_regression(2, 0, 3, begin - 1, begin);
R_blackfuzhi(parameterA, parameterB, 0, 60);
}
}
else if (type == 8)
{
flag_blank_out_huandao = 0;
}
}
}
//左环岛右线结束
//计算宽度开始
void cakkuandu()
{
int i;
for (i = 0; i <= 69; i++)
{
calkuan[i] = 0;
}
for (i = 0; i <= break_hangshu; i++)
{
calkuan[i] = L_black[i] - R_black[i];
}
for (i = 0; i <= break_hangshu; i++)
{
//SetText("行数+宽度" + " " + i + " " + calkuan[i]);
}
}
//计算宽度结束
//找环岛拐点开始
int flag_find_huan_leftout_point;
int left_turn_out [2];
void findhuan_leftout_point(int start_point, int end_point, int RoadName) //找换到左下拐点 为左环岛而修改
{
int j;
if (RoadName == ROUNDISLAND)
{
//setText用户自定义("l_start"+ l_start);
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if (L_black[j]-L_black[j -1]<=0 && L_black[j] - L_black[j + 1] <= 0 && L_black[j] - L_black[j + 2] <= 0 && L_black[j] - L_black[j - 2] <= 0
&&( L_black[j] - L_black[j + 3] <= -1 && L_black[j] - L_black[j - 3] <= -1)
&& leftflag[j] == 1 && leftflag[j-1] == 1 && leftflag[j-2] == 1 && leftflag[j-3] == 1
&& leftflag[j + 1] == 1 && leftflag[j + 2] == 1 && leftflag[j + 3] == 1)
{
int record = 0;
int i;
if (L_black[j - 1] == L_black[j - 2] && L_black[j - 2] == L_black[j - 3]) j = j - 1;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
left_turn_out[0] = (int)(record);//数组里面没有第0行
left_turn_out[1] = L_black[record];
flag_find_huan_leftout_point = 1;
break;
}
}
}
}
int flag_find_huan_rightout_point;
int right_turn_out [2];
void findhuan_rightout_point(int start_point, int end_point, int RoadName) //找环岛右下拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j - 1]-R_black[j - 2]>=0 &&R_black[j-1]-R_black[j - 3]>=0 && R_black[j - 1] - R_black[j] >= 0 && R_black[j - 1] - R_black[j+1] >= 0
&& (R_black[j - 1] - R_black[j-4] >= 1&&R_black[j - 1] - R_black[j + 2] >= 1)
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && rightflag[j+1] == 1 && rightflag[j -3] == 1 && rightflag[j -4] == 1 && rightflag[j +2] == 1
&& R_black[j - 1]<130&& R_black[j - 1] >10)
{
int record = 0;
int i;
record = j;
for (i = j; i >= 1; i--)
{
if (R_black[i] == R_black[i - 1]) record = i - 1;
if (R_black[i] != R_black[i - 1]) break;
}
right_turn_out[0] = (int)(record);//数组里面没有第0行
right_turn_out[1] = R_black[record];
flag_find_huan_rightout_point = 1;
break;
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
void findhuan_rightdown_point(int start_point, int end_point, int RoadName) //找环岛右下拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j]-R_black[j - 1]>= 0 && R_black[j]- R_black[j - 2]>=0 &&R_black[j] - R_black[j+1] >=0&& R_black[j] - R_black[j + 2] >= 0
&&( R_black[j] - R_black[j - 1]<=5 || R_black[j] - R_black[j-2] <= 5)&&j<=break_hangshu-3
&& (R_black[j] - R_black[j - 2] >= 1 || R_black[j] - R_black[j - 3] >= 1 || (j>=4&&R_black[j] - R_black[j - 4] >= 1) || (j >= 5&&R_black[j] - R_black[j - 4] >= 1))
&& (R_black[j] - R_black[j + 2] >= 1 || R_black[j] - R_black[j + 3] >= 1 || R_black[j] - R_black[j + 4] >= 1 || R_black[j] - R_black[j + 5] >= 1 || R_black[j] - R_black[j + 6] >= 1)
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && (rightflag[j+1] == 1||R_black[j+1]==0))
{
if (R_black[j] < 175)
{
int record = 0;
//int i;
record = j;
//for (i = j; i >= 1; i--)
//{
// if (R_black[i] == R_black[i - 1]) record = i - 1;
// if (R_black[i] != R_black[i - 1]) break;
//}
right_turn_down[0] = (int)(record);//数组里面没有第0行
right_turn_down[1] = R_black[record];
flag_find_huan_rightdown_point = 1;
// SetText("找到环岛右下拐点 找到右下拐点" + right_turn_down[0] + " " + right_turn_down[1]);
break;
}
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
void findhuan100_rightdown_point(int start_point, int end_point, int RoadName) //找环岛右下拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j] - R_black[j - 1] >= 0 && R_black[j] - R_black[j - 2] >= 0 && R_black[j] - R_black[j + 1] >= 0 && R_black[j] - R_black[j + 2] >= 0
&& R_black[j] - R_black[j - 1] <= 5 && R_black[j-1] - R_black[j-2] <= 5
&& (R_black[j] - R_black[j - 2] >= 1 || R_black[j] - R_black[j - 3] >= 1)
&& (R_black[j] - R_black[j + 2] >= 1 || R_black[j] - R_black[j + 3] >= 1 || R_black[j] - R_black[j + 4] >= 1 || R_black[j] - R_black[j + 5] >= 1 || R_black[j] - R_black[j + 6] >= 1)
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && rightflag[j + 1] == 1)
{
if (R_black[j] < 175)
{
int record = 0;
record = j;
//for (i = j; i >= 1; i--)
//{
// if (R_black[i] == R_black[i - 1]) record = i - 1;
// if (R_black[i] != R_black[i - 1]) break;
//}
right_turn_down[0] = (int)(record);//数组里面没有第0行
right_turn_down[1] = R_black[record];
flag_find_huan_rightdown_point = 1;
break;
}
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
void findhuan_leftdown_point(int start_point, int end_point, int RoadName) //找换到左下拐点 为左环岛而修改
{
int j;
if (RoadName == ROUNDISLAND)
{
//setText用户自定义("l_start"+ l_start);
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if (My_Abs(L_black[j - 1], L_black[j - 2]) <=4 && My_Abs(L_black[j], L_black[j - 1]) <= 5 && (L_black[j + 1] - L_black[j] >= 2)&& (L_black[j + 2] - L_black[j] >= 2)
&& leftflag[j - 2] == 1 && leftflag[j - 1] == 1 && leftflag[j] == 1)
{
int record = 0;
int i;
if (L_black[j - 1] == L_black[j - 2] && L_black[j - 2] == L_black[j - 3]) j= j - 1;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
left_turn_down[0] = (int)(record);//数组里面没有第0行
left_turn_down[1] = L_black[record];
flag_find_huan_leftdown_point = 1;
// SetText("找到环岛左下拐点 找到左下拐点" + left_turn_down[0] + " " + left_turn_down[1]);
break;
}
}
}
}
void findhuan_rightmiddle_point(int start_point, int end_point) //找环岛右中拐点 此函数待调整
{
int i;
if ((huandao_memory == 4 || huandao_memoryforleft == 4) && break_hangshu >= 10) end_point = break_hangshu - 10;
for (i = start_point; i <= end_point; i++)
{
//找you拐点
if (i >= 8 && (R_black[i + 1] - R_black[i] <= 0) && (R_black[i + 2] - R_black[i] <= 0)&&
(R_black[i - 1] - R_black[i] <= 0) && (R_black[i - 2] - R_black[i] <= 0) && (R_black[i - 3] - R_black[i] <= 0) && (R_black[i - 4] - R_black[i] <= 0) && ( (R_black[i - 5] - R_black[i] <= -1) || (R_black[i - 6] - R_black[i] <= -1) || (R_black[i - 7] - R_black[i] <= -1) || (R_black[i - 8] - R_black[i] <= -1))
&& rightflag[i - 2] == 1 && rightflag[i - 1] == 1 && rightflag[i] == 1 && rightflag[i + 1] == 1 && rightflag[i + 2] == 1
)
{
right_turn_middle[0] = (int)(i);
right_turn_middle[1] = R_black[i];
flag_find_huan_rightmiddle_point = 1;
break;
}
}
}
void findhuan_leftmiddle_point(int start_point, int end_point) //找环岛左中拐点
{
int i;
for (i = start_point; i <= end_point; i++)
{
//找左拐点
if (i >= 8 && (L_black[i + 1] - L_black[i] >= 0) && (L_black[i + 2] - L_black[i] >= 0 ) && (L_black[i - 1] - L_black[i] >= 0) && (L_black[i - 2] - L_black[i] >= 0) && (L_black[i - 3] - L_black[i] >= 0) && (L_black[i - 4] - L_black[i] >= 1 || L_black[i - 5] - L_black[i] >= 1)
&& (leftflag[i - 2] == 1 || leftflag[i - 3] == 1) && leftflag[i - 1] == 1 && leftflag[i] == 1 && leftflag[i + 1] == 1 && (leftflag[i + 2] == 1 || leftflag[i + 3] == 1)&&i<=break_hangshu-4)
{
int record = 0;
int j;
record = i;
for (j= i; j >= 1; j--)
{
if (L_black[j] == L_black[j - 1]) record = j - 1;
if (L_black[j] != L_black[j - 1]) break;
}
if (L_black[record] <= 178)
{
left_turn_middle[0] = (int)(record);
left_turn_middle[1] = L_black[record];
flag_find_huan_leftmiddle_point = 1;
break;
}
}
}
}
void findhuan_rightup_point(int start_point, int end_point, int RoadName)// 找环岛右上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((R_black[j - 1] - R_black[j - 2]) >= 4 && My_Abs(R_black[j], R_black[j - 1]) <= 4 && My_Abs(R_black[j + 1], R_black[j]) <= 4
&& rightflag[j - 1] == 1 && rightflag[j] == 1 && rightflag[j + 1] == 1 && My_Abs(R_black[j - 1], LCenter[j - 1]) <= 45 && My_Abs(R_black[j], LCenter[j]) <= 45
&& (huandao_memory != 4 || (huandao_memory == 4 && R_black[j - 1] >= 20&&j<break_hangshu-2)))//
{
right_turn_up[0] = (int)(j - 1);
right_turn_up[1] = R_black[j - 1];
flag_find_huan_rightup_point = 1;
// SetText("找到环岛右上拐点 找到右上拐点" + right_turn_up[0] + " " + right_turn_up[1]);
break;
}
}
}
}
void findhuan_leftup_point(int start_point, int end_point, int RoadName) //找环岛左上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//左上拐点
if ((L_black[j - 2] - L_black[j - 1]) >= 6 && My_Abs(L_black[j], L_black[j - 1]) <= 3 && My_Abs(L_black[j + 1], L_black[j]) <= 3
&& leftflag[j - 1] == 1 && leftflag[j] == 1 && leftflag[j + 1] == 1 && My_Abs(L_black[j - 1], LCenter[j - 1]) <= 45 && My_Abs(L_black[j], LCenter[j]) <= 45
&&((huandao_memoryforleft!=4|| (huandao_memoryforleft == 4&& L_black[j - 1]<=160&&j<=break_hangshu-2))))
{
left_turn_up[0] = (int)(j - 1);//数组里面没有第0行
left_turn_up[1] = L_black[j - 1];
flag_find_huan_leftup_point = 1;
// SetText("找到环岛左上拐点77 找到左上拐点77 " + left_turn_up[0] + " " + left_turn_up[1]);
break;
}
}
}
}
void findhuan_rightup_point77(int start_point, int end_point, int RoadName)// 找环岛右上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((R_black[j - 1] - R_black[j - 2]) >= 4 && My_Abs(R_black[j], R_black[j - 1]) <= 4 && My_Abs(R_black[j + 1], R_black[j]) <= 4
&& rightflag[j - 1] == 1 && rightflag[j] == 1 && rightflag[j + 1] == 1)
{
right_turn_up[0] = (int)(j - 1);
right_turn_up[1] = R_black[j - 1];
flag_find_huan_rightup_point = 1;
// SetText("找到环岛右上拐点77 找到右上拐点77" + right_turn_up[0] + " " + right_turn_up[1]);
break;
}
}
}
}
void findhuan_rightup_point44(int start_point, int end_point, int RoadName)// 找环岛右上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
if (R_black[j] >= 20)
{ //如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((R_black[j - 1] - R_black[j - 2]) >= 4 && My_Abs(R_black[j], R_black[j - 1]) <= 4 && My_Abs(R_black[j + 1], R_black[j]) <= 4
&& rightflag[j - 1] == 1 && rightflag[j] == 1 && rightflag[j + 1] == 1 && My_Abs(R_black[j - 1], LCenter[j - 1]) <= 60 && My_Abs(R_black[j], LCenter[j]) <= 60&&(huandao_memory!=4||(huandao_memory==4&&j<=break_hangshu-2)))
{
right_turn_up[0] = (int)(j - 1);
right_turn_up[1] = R_black[j - 1];
flag_find_huan_rightup_point = 1;
// SetText("找到环岛右上拐点 找到右上拐点44" + right_turn_up[0] + " " + right_turn_up[1]);
break;
}
}
}
}
}
void findhuan_leftup_point77(int start_point, int end_point, int RoadName) //找环岛左上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//左上拐点
if ((L_black[j - 2] - L_black[j - 1]) >= 4 && My_Abs(L_black[j], L_black[j - 1]) <= 3 && My_Abs(L_black[j + 1], L_black[j]) <= 3
&& leftflag[j - 1] == 1 && leftflag[j] == 1 && leftflag[j + 1] == 1)
{
left_turn_up[0] = (int)(j - 1);//数组里面没有第0行
left_turn_up[1] = L_black[j - 1];
flag_find_huan_leftup_point = 1;
break;
}
}
}
}
//找环岛拐点结束
void advanced_regression(int type, int startline1, int endline1, int startline2, int endline2)
{
int i = 0;
int sumlines1 = endline1 - startline1;
int sumlines2 = endline2 - startline2;
int sumX = 0;
int sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += LCenter[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += LCenter[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1) //拟合左线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += L_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += L_black[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2) //拟合右线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += R_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += R_black[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
}
void regression(int type, int startline, int endline)//最小二乘法拟合曲线，分别拟合中线，左线，右线,type表示拟合哪几条线 xy 颠倒
{
int i = 0;
int sumlines = endline - startline;
int sumX = 0;
int sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += LCenter[i];
}
if (sumlines != 0)
{
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
}
else
{
averageX = 0; //x的平均值
averageY = 0; //y的平均值
}
for (i = startline; i < endline; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1)//拟合左线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += L_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2)//拟合右线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += R_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
}
void leftlinefuzhi(float A, float B, int8 start_point, int8 end_point)
{
int8 m;
for (m = start_point; m <= end_point; m++)
{
if((B * m + A)>=255) L_black[m]=255;
if((B * m + A)<=0) L_black[m]=0;
else if(0<(B * m + A)&&(B * m + A)<255)L_black[m] = (int8)(B * m + A);
}
}
void R_blackfuzhi(float A, float B, int8 start_point, int8 end_point)
{
int8 m;
for (m = start_point; m <= end_point; m++)
{
if ((B * m + A) < 0) {R_black[m] = 0; }
else if((B * m + A) >=0&&(B * m + A)<=255) R_black[m] = (int8)(B * m + A);
if((B * m + A)>=255) R_black[m]=255;
}
}
void zhongxianfuzhi( int8 start_point, int8 end_point)
{
int8 m;
for (m = start_point; m <= end_point; m++)
{
LCenter[m] = (int8)(L_black[m]/2 + R_black[m]/ 2);
}
}
void monileftfuzhi(float A, float B, int start_point, int end_point)
{
int m;
for (m = start_point; m <= end_point; m++)
{
if((B * m + A)>=255) monileft[m]=255;
if((B * m + A)<=0) monileft[m]=0;
else if(0<(B * m + A)&&(B * m + A)<255)monileft[m] = (int)(B * m + A);
}
}
void monirightfuzhi(float A, float B, int start_point, int end_point)
{
int m;
for (m = start_point; m <= end_point; m++)
{
if((B * m + A)>=255) moniright[m]=255;
if((B * m + A)<=0) moniright[m]=0;
else if(0<(B * m + A)&&(B * m + A)<255)moniright[m] = (int)(B * m + A);
}
}
void monizhongfuzhi(float A, float B, int start_point, int end_point)
{
int m;
for (m = start_point; m <= end_point; m++)
{
if ((B * m + A) >= 255) monimiddle[m] = 255;
if ((B * m + A) <= 0) monimiddle[m] = 0;
else if (0 < (B * m + A) && (B * m + A) < 255) monimiddle[m] = (int)(B * m + A);
}
}
总结
1.配合上辅助函数就是完整代码，如有不懂请留言哦。