原创博客,转载请注明出处:http://write.blog.csdn.net/postlist
除了直线,在激光雷达获取的数据中,最重要的就是圆弧了,圆弧的位置确定本生没有直线的精度高,
因此不适合用作定位的基准,但是机器人在执行动作时,需要确定圆弧的位置,或则根据圆弧确定目标是
什么或者目标的位置。
圆弧的检测包括圆弧的位置(x,y)和大小r,常用的方法包括Hough变换和最小二乘法拟合。
一般圆弧位置检测的精度比较低,不能作为定位的标准,不过可以确定机器人和目标之间的位置。
1、Hough变换
圆弧检测之前都需要对数据进行分割,将一系列的点分割成不同的区域,然后计算圆弧的位置。Hough变换
不需要知道某区域是否有圆弧,以类似于投票的机制,某参数获得的票数越多,则存在圆弧的可能性越大,
大于某阈值时,则可以认为该处存在圆弧。
x - a = r*cos(theta)
y - b = r*sin(theta)
对于每一个点x y,有无数个点满足上式,即有无数个圆在经过该点的,每个圆对应一组(a,b,r)参数,设立一
个票箱,里面有所有可能的(a,b,r)参数,当某组参数出现一次,就将该票箱中的票数加1,所有的点都扫描之
后,查看票箱,票数最多的点即是圆出现概率最大的情况。此时应该设定一个阈值,如果最多的票数小于该阈值,
则认为不存在圆,否则认为有圆存在。
//这是一个简化的Hough圆算法,假设半径已知的情况,如果想看完整的Hough圆变换,查看另一篇博客:
http://blog.csdn.net/renshengrumenglibing/article/details/7250146
int HoughArc(int X[] , int Y[] , int Cnt ,int r, ArcPara * Arc){ vector<iPoint>center; vector<int>VoteCnt; double theta; int a,b; int minA,maxA,minB,maxB; int VotedFlag = 0; double deltaTheta = PI/180;//间隔1度 double startAngle = 150.0*PI/180; double endAngle = PI*2 + PI/6; center.clear(); VoteCnt.clear(); minA = maxA = X[0] - r; minB = maxB = X[0]; //theta = 0 //计算a,b的最小和最大值 for (int i = 0; i < Cnt;i++) { for (theta = startAngle; theta < endAngle;theta += deltaTheta) { a = (int)(X[i] - r*cos(theta) + 0.5); b = (int)(Y[i] - r*sin(theta) + 0.5); if (a > maxA) { maxA = a; }else if (a < minA) { minA = a; } if (b > maxB) { maxB = b; }else if (b < minB) { minB = b; } } } //确定a,b的范围之后,即确定了票箱的大小 int aScale = maxA - minA + 1; int bScale = maxB - minB + 1; int *VoteBox = new int[aScale*bScale]; //VoteBox初始化为0 for (int i = 0; i < aScale*bScale;i++) { VoteBox[i] = 0; } //开始投票 for (int i = 0; i < Cnt;i++) { //printf("%d ",i); for (theta = startAngle; theta < endAngle;theta += deltaTheta) { a = (int)(X[i] - r*cos(theta) + 0.5); b = (int)(Y[i] - r*sin(theta) + 0.5); VoteBox[(b - minB)*aScale + a - minA] = VoteBox[(b - minB)*aScale + a - minA] + 1; } } //筛选票箱 int VoteMax = 0; int VoteMaxX,VoteMaxY; for (int i = 0; i < bScale ;i++) { for (int j = 0; j < aScale ;j++) { if (VoteBox[i*aScale + j] > VoteMax) { VoteMax = VoteBox[i*aScale + j]; VoteMaxY = i; VoteMaxX = j; } } } int Count = 0; printf("VoteMax: %d",VoteMax); for (int i = 0; i < bScale ;i++) { for (int j = 0; j < aScale ;j++) { if (VoteBox[i*aScale + j] >= VoteMax) { Count++; } } } printf(" %d \n",Count); //释放内存 delete [] VoteBox; if (VoteMax > 3) { Arc->center.x = VoteMaxX + minA; Arc->center.y = VoteMaxY + minB; Arc->r = r; return 1; }else { return 0; } return 1; }
2、最小二乘法拟合
最小二乘法拟合则需要先判定是否是圆弧,是圆弧才能进行拟合,否则拟合的结果肯定是不准的。
但是目前直线的判定可以通过多边形拟合寻角点、拟合后看个点与直线距离等方法来判定,圆弧并没有
很好的判定方法,起码我目前是没有发现的。