激光雷达学习笔记（五）圆弧

原创博客，转载请注明出处：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

[cpp]  view plain  copy

1. int HoughArc(int X[] , int Y[] , int Cnt ,int r, ArcPara * Arc){  
2.     vector<iPoint>center;  
3.     vector<int>VoteCnt;  
4.     double theta;  
5.     int a,b;  
6.     int minA,maxA,minB,maxB;  
7.     int VotedFlag = 0;  
8.     double deltaTheta = PI/180;//间隔1度  
9.     double startAngle = 150.0*PI/180;  
10.     double endAngle = PI*2 + PI/6;  
11.     center.clear();  
12.     VoteCnt.clear();  
13.     minA = maxA = X[0] - r;  
14.     minB = maxB = X[0]; //theta = 0  
15.     //计算a，b的最小和最大值  
16.     for (int i = 0; i < Cnt;i++)  
17.     {  
18.         for (theta = startAngle; theta < endAngle;theta += deltaTheta)  
19.         {  
20.             a = (int)(X[i] - r*cos(theta) + 0.5);  
21.             b = (int)(Y[i] - r*sin(theta) + 0.5);  
22.             if (a > maxA)  
23.             {  
24.                 maxA = a;  
25.             }else if (a < minA)  
26.             {  
27.                 minA = a;  
28.             }  
29.   
30.             if (b > maxB)  
31.             {  
32.                 maxB = b;  
33.             }else if (b < minB)  
34.             {  
35.                 minB = b;  
36.             }  
37.   
38.         }  
39.     }  
40.     //确定a，b的范围之后，即确定了票箱的大小  
41.     int aScale = maxA - minA + 1;  
42.     int bScale = maxB - minB + 1;  
43.   
44.     int *VoteBox = new int[aScale*bScale];  
45.     //VoteBox初始化为0  
46.     for (int i = 0; i < aScale*bScale;i++)  
47.     {  
48.         VoteBox[i] = 0;  
49.     }  
50.     //开始投票  
51.     for (int i = 0; i < Cnt;i++)  
52.     {  
53.         //printf("%d  ",i);  
54.         for (theta = startAngle; theta < endAngle;theta += deltaTheta)  
55.         {  
56.   
57.             a = (int)(X[i] - r*cos(theta) + 0.5);  
58.             b = (int)(Y[i] - r*sin(theta) + 0.5);     
59.             VoteBox[(b - minB)*aScale + a - minA] = VoteBox[(b - minB)*aScale + a - minA] + 1;  
60.         }  
61.     }  
62.   
63.     //筛选票箱  
64.     int VoteMax = 0;  
65.     int VoteMaxX,VoteMaxY;  
66.     for (int i = 0; i < bScale ;i++)  
67.     {  
68.         for (int j = 0; j < aScale ;j++)  
69.         {  
70.             if (VoteBox[i*aScale + j] > VoteMax)  
71.             {  
72.                 VoteMax = VoteBox[i*aScale + j];  
73.                 VoteMaxY = i;  
74.                 VoteMaxX = j;  
75.             }  
76.         }  
77.     }  
78.       
79.     int Count = 0;  
80.     printf("VoteMax: %d",VoteMax);  
81.     for (int i = 0; i < bScale ;i++)  
82.     {  
83.         for (int j = 0; j < aScale ;j++)  
84.         {  
85.             if (VoteBox[i*aScale + j] >= VoteMax)  
86.             {  
87.                 Count++;  
88.             }  
89.         }  
90.     }  
91.     printf("   %d \n",Count);  
92.     //释放内存  
93.     delete [] VoteBox;  
94.     if (VoteMax > 3)  
95.     {  
96.         Arc->center.x = VoteMaxX + minA;  
97.         Arc->center.y = VoteMaxY + minB;  
98.         Arc->r = r;  
99.         return 1;  
100.     }else {  
101.         return 0;  
102.     }  
103.     return 1;  
104. }  

2、最小二乘法拟合

最小二乘法拟合则需要先判定是否是圆弧，是圆弧才能进行拟合，否则拟合的结果肯定是不准的。

但是目前直线的判定可以通过多边形拟合寻角点、拟合后看个点与直线距离等方法来判定，圆弧并没有

很好的判定方法，起码我目前是没有发现的。

最新代码下载：http://download.csdn.net/detail/renshengrumenglibing/5100757