智能车图像处理16-三叉路口的判断
前言
这篇文章主要讲述图像处理中如何判断三叉路口并进行补线操作。
一、函数主体
void threeroad()
{
int hang;
int i;
int end = 0;
for (hang = 1; hang < 69; hang++) //去掉杂点
{
if (LCenter[hang] == 0 && LCenter[hang + 1] == 0)
{
end = hang - 1;
break;
}
if (My_Abs(LCenter[hang], LCenter[hang + 1]) > 20 && hang > 5)
{
end = hang - 1;
break;
}
}
countleft10 = 0;
countright10 = 0;
threekuanduEQ = 0;
secondleft = 0;
firstleft = 0;
//三叉先行找一下拐点
int start=end;
for (i = end; i < 60; i++)
{
if (Pixels[i][ 93] == 0)
{
start = i;
break;
}
}
int allwhite = 0;
int startallright = 0;
for (i = 45; i > 0; i--)
{
if (Pixels[i + 3][2] == 0 && Pixels[i + 2][ 2] == 0 && Pixels[i + 1][2] == 1 && Pixels[i][ 2] == 1)
{
startallright = i+1;
break;
}
}
for (i = startallright; i > 0; i--)
{
if (Pixels[i][ 0] == 0)
{
allwhite = 1;
break;
}
}
int allwhiteright = 0;
int startallleft = 0;
for (i = 30; i > 0; i--)
{
if ( Pixels[i + 2][ 185] == 0 && Pixels[i + 1][ 185] == 1 )
{
startallleft = i + 1;
break;
}
}
for (i = startallleft; i > 0; i--)
{
if (Pixels[i][ 185] == 0)
{
allwhiteright = 1;
break;
}
}
/*
敲黑板!下面是三叉路口的思路，有助于理解代码
首先:明确十字与三叉的区别
大家可以想一下十字和三叉十字路口。十字路口的图像是一条白色直道直通到视角尽头，而三叉的白色直道道整个图像的中央就分叉开来了。
其次:三叉路口辅助判断条件含义:
(1)入口的两个下拐点:三叉和十字的下拐点判断条件是有细微差别的，大家可以具体对比一下。
(2)endmiddle:就是白色直道的最终行数:我有四十行图像，十字的endmiddle会有37、38，而三叉的endmiddle只有不超过34这样出现分叉。
(3)endmiddle改进建议:
这里的取法是找到左右下拐点取中值向上延伸，如果想要改进这个条件的准确度。建议是取左右拐点较小的行数，从0到该行用最小二乘法拟合往下延伸寻找endmiddle。
(辅助条件判断方法在另一篇文章中)
最后:
判断思路:
(1)非环岛状态
(2)最有边线的第0行必须在视野范围内(1-185)。因为在十字转弯的时候，很多情况下左下或者右下是全白的，这一个条件可以有效的排除部分十字。但也意味着想要判断三叉，必须要车身是正的。
(因为斜入十字和三叉特别像，大家估计也发现了)
(3)找到左右下拐点，且左右下拐点行数差必须要在一定范围之内。(车正 基本上左右下拐点差个5行最多了)
(4)endmiddle小于37行(我一共42行，大家自己调整这个值)
(5)
calkuan 宽度数组 在左右下拐点行数较高的那行向上1 2 3 行宽度要大于100.
(因为三叉后面呈现分叉状态,宽度会变大不少，宽度太小就不认为是三叉。)
注:状态1-6 为一个完整的入三叉到出三叉过程，因为我的车要跑两圈 有状态7-12
*/
//147 10 25811
if (bigup != 0&& haveleftku == 0&&threemode ==0&&threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 110 && calkuan[bigup + 2] > 110
&& calkuan[bigup + 3] > 110 && calkuan[bigup] > 100
&& (endmidddle<=37&& endmidddle !=0)&& huandao_memory==0&&huandao_memoryforleft==0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 15&&secondmid==0&&firstmid==0&&se_thrmid==0
)//
threemode = 1;//进入状态1 找点拉线
if ((haveleftku == 0 && threemode == 1 && R_black[0]==0&&
L_black[0]==186)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)//&&(threerightdownflag==0|| threeleftdownflag==0)
) threemode = 2;//进入状态2 右下角拉线
if (haveleftku == 0 && threemode == 2 &&(R_black[0]!= 0|| L_black[0] != 186) && (threerightdownflag == 0 || threeleftdownflag == 0)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
) threemode = 3; //进入状态3 正常跑
if (bigup != 0 && haveleftku == 0 && threemode == 3 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 100 && calkuan[bigup + 2] > 100
&& calkuan[bigup + 3] > 100 && calkuan[bigup] > 100
&& (endmidddle <= 37 && endmidddle != 0)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 17 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
) threemode = 4; //进入状态四 同1 找点拉线
if (haveleftku == 0 && threemode == 4 && R_black[0] == 0 && L_black[0] == 186
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
)//&& (threerightdownflag == 0 || threeleftdownflag == 0)
{
threemode = 5; //进入状态5 同2 右下角拉线
}
if (haveleftku == 0 && threemode == 5 && (R_black[0] != 0 || L_black[0] != 186) && (threerightdownflag == 0 || threeleftdownflag == 0)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
)
{
threemode = 6; //进入6 正常跑状态
}
if (bigup != 0 && haveleftku == 0 && threemode == 6 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 110 && calkuan[bigup + 2] > 110
&& calkuan[bigup + 3] > 110 && calkuan[bigup] > 100
&& (endmidddle <= 37 && endmidddle != 0)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 15 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
) threemode = 7;//进入状态7 找左下拐点拉线
if (haveleftku == 0 && threemode == 7&&R_black[0] == 0 &&
L_black[0] == 186
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
)// && (threerightdownflag == 0 || threeleftdownflag == 0)
threemode =8;//进入状态8 直接拉线
if (haveleftku == 0 && threemode == 8 &&( L_black[0]!=186||R_black[0]!=0) && (threerightdownflag == 0 || threeleftdownflag == 0)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
) threemode = 9; //进入状态9 正常跑
// threemode = 9;
if (bigup != 0 && haveleftku == 0 && threemode == 9 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 100 && calkuan[bigup + 2] > 100
&& calkuan[bigup + 3] > 100 && calkuan[bigup] > 100
&& (endmidddle <= 37 && endmidddle != 0)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 17 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
) threemode = 10;//进入状态10 同6 找左下挂点拉线
if (haveleftku == 0 && threemode == 10 && R_black[0] == 0 &&
L_black[0] == 186
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
)//&& (threerightdownflag == 0 || threeleftdownflag == 0)
{
threemode = 11;//进入状态11 同7 左下角拉线
}
if (haveleftku == 0 && threemode == 11 && (L_black[0] != 186 || R_black[0] != 0) && (threerightdownflag == 0 || threeleftdownflag == 0)
&& huandao_memory == 0 && huandao_memoryforleft == 0
&& turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
) //进入状态12 继续跑
{
threemode = 12; //无状态什么都不做继续跑
}
threemodeop(threemode);
// threemodeop(threemode);
if (givetime != 1)
{
threeroadextrale = threeroadextra;
threeroadextrari = threeroadextra;
givetime = 1;
}
if (threeroadextrale != 0 && threemode == 6)
{
threemode = 0;
threeroadextrale = threeroadextrale - 1;
}
if(threeroadextrari != 0&&threemode==12)
{
threemode = 6;
threeroadextrari = threeroadextrari - 1;
}
}
//三岔路口结束
2.辅助函数
//求两数之差绝对值开始
int My_Abs(int a, int b)
{
if ((a - b) > 0)
return ((int)(a - b));
else return ((int)(b - a));
}
//求两数之差绝对值结束
//三叉路口操作开始
void threemodeop(int modetype)
{
int i;
int j;
if ((modetype == 1|| modetype ==4)&& threerightdown[1]>=1&& LCenter[end]>=1) //
{
int endmidddle=0;
for (i = 0; i < 60; i++)
{
if (Pixels[i][ 93] == 0)
{
endmidddle = i;
break;
}
}
R_black[threerightdown[0] - 1] = (unsigned char)(threerightdown[1] - 1);
R_black[threerightdown[0]]=(unsigned char)(threerightdown[1]);
if (endmidddle <= 1) endmidddle = 1;
R_black[endmidddle - 1] = 93;
R_black[endmidddle] = 94;
advanced_regression34(2, threerightdown[0] - 1, threerightdown[0], endmidddle - 1, endmidddle);
R_blackfuzhi(parameterA, parameterB, threerightdown[0] - 1, 50);
for (i = 0; i < 60; i++)
{
if (L_black[i] < 130) L_black[i] = 186;
}
for (j = 0; j < 70; j++)
{
LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
}
for (i = threerightdown[0]-1; i < 60; i++)
{
if (LCenter[i]<93) LCenter[i] = 93;
}
}
if (modetype == 2||modetype==5) //
{
int endmidddle = 0;
for (i = 0; i < 60; i++)
{
if (Pixels[i][93] == 0)
{
endmidddle = i;
break;
}
}
if (endmidddle <= 1) endmidddle = 1;
R_black[endmidddle - 1] = 90;
R_black[endmidddle] = 93;
advanced_regression34(2, 0, 1, endmidddle - 1, endmidddle);
R_blackfuzhi(parameterA, parameterB, 0, 50);
for (i = 0; i < 60; i++)
{
if (L_black[i] < 130) L_black[i] = 186;
}
for (j = 0; j < 70; j++)
{
LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
}
for (i = 0; i < 60; i++)
{
if (LCenter[i] < 93) LCenter[i] = 93;
}
}
if ((modetype == 7|| modetype == 10) && threeleftdown[1] >= 1 && LCenter[end] >= 1) //
{
int endmidddle = 0;
for (i = 0; i < 60; i++)
{
if (Pixels[i][ 93] == 0)
{
endmidddle = i;
break;
}
}
if (endmidddle <= 1) endmidddle = 1;
L_black[threeleftdown[0] - 1] = (unsigned char)(threeleftdown[1] - 1);
L_black[threeleftdown[0]] = (unsigned char)(threeleftdown[1]);
if (endmidddle <= 1) endmidddle = 1;
L_black[endmidddle - 1] = 93;
L_black[endmidddle] = 91;
advanced_regression34(1, threeleftdown[0] - 1, threeleftdown[0], endmidddle - 1, endmidddle);
leftlinefuzhi(parameterA, parameterB, threeleftdown[0] - 1, 50);
for (i = 0; i < 60; i++)
{
if (R_black[i] > 60) R_black[i] = 0;
}
for (j = 0; j < 70; j++)
{
LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
}
for (i = threeleftdown[0] - 1; i < 60; i++)
{
if (LCenter[i] > 93) LCenter[i] = 93;
}
}
if (modetype ==8|| modetype==11) //
{
int endmidddle = 0;
for (i = 0; i < 60; i++)
{
if (Pixels[i][93] == 0)
{
endmidddle = i;
break;
}
}
if (endmidddle <= 1) endmidddle = 1;
L_black[endmidddle - 1] = 93;
L_black[endmidddle] = 90;
advanced_regression34(1,0,1, endmidddle - 1, endmidddle);
leftlinefuzhi(parameterA, parameterB, 0, 50);
for (i = 0; i < 60; i++)
{
if (R_black[i] > 60) R_black[i] = 0;
}
for (j = 0; j < 70; j++)
{
LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
}
for (i = 0; i < 60; i++)
{
if (LCenter[i] > 93) LCenter[i] = 93;
}
}
}
//三岔路口操作结束
void advanced_regression34(int type, int startline1, int endline1, int startline2, int endline2)//最小二乘法拟合曲线，分别拟合中线，左线，右线,type表示拟合哪几条线
{
int i = 0;
int sumlines = endline1 - startline1 + endline2 - startline2;
int sumX = 0;
int sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
for (i = startline1; i < endline1; 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 = startline1; i < endline1; 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 = startline1; i < endline1; i++)
{
sumX += i;
sumY += L_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += L_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)sumX / sumlines; //x的平均值
averageY = (float)sumY / sumlines; //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)//拟合右线
{
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += R_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += R_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)sumX / sumlines; //x的平均值
averageY = (float)sumY / sumlines; //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;
}
else if (type == 3) //左下拉右上
{
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += L_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += R_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)sumX / sumlines; //x的平均值
averageY =(float) sumY / sumlines; //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 += (R_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 == 4) //右下拉左上
{
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += R_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += L_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX =(float) sumX / sumlines; //x的平均值
averageY = (float)sumY / sumlines; //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 += (L_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);
}
}
总结
1.配合上辅助函数就是完整三岔路口判断、补线代码，如有不懂请留言哦。