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

出处：http://write.blog.csdn.net/postlist

除了直线，在激光雷达获取的数据中，最重要的就是圆弧了，圆弧的位置确定本生没有直线的精度高，

因此不适合用作定位的基准，但是机器人在执行动作时，需要确定圆弧的位置，或则根据圆弧确定目标是

什么或者目标的位置。

圆弧的检测包括圆弧的位置（x,y）和大小r，常用的方法包括Hough变换和最小二乘法拟合。
一般圆弧位置检测的精度比较低，不能作为定位的标准，不过可以确定机器人和目标之间的位置。

1、Hough变换

圆弧检测之前都需要对数据进行分割，将一系列的点分割成不同的区域，然后计算圆弧的位置。Hough变换

不需要知道某区域是否有圆弧，以类似于投票的机制，某参数获得的票数越多，则存在圆弧的可能性越大， www.it165.net

大于某阈值时，则可以认为该处存在圆弧。

x - a = r*cos(theta)

y - b = r*sin(theta)

对于每一个点x y，有无数个点满足上式，即有无数个圆在经过该点的，每个圆对应一组（a,b,r）参数，设立一

个票箱，里面有所有可能的（a,b,r）参数，当某组参数出现一次，就将该票箱中的票数加1，所有的点都扫描之

后，查看票箱，票数最多的点即是圆出现概率最大的情况。此时应该设定一个阈值，如果最多的票数小于该阈值，

则认为不存在圆，否则认为有圆存在。

//这是一个简化的Hough圆算法，假设半径已知的情况

 

view source print ?

001. int HoughArc(int X[] , int Y[] , int Cnt ,int r, ArcPara * Arc){

002. vector<iPoint>center;

003. vector<int>VoteCnt;

004. double theta;

005. int a,b;

006. int minA,maxA,minB,maxB;

007. int VotedFlag = 0;

008. double deltaTheta = PI/180;//间隔1度

009. double startAngle = 150.0*PI/180;

010. double endAngle = PI*2 + PI/6;

011. center.clear();

012. VoteCnt.clear();

013. minA = maxA = X[0] - r;

014. minB = maxB = X[0]; //theta = 0

015. //计算a，b的最小和最大值

016. for (int i = 0; i < Cnt;i++)

017. {

018. for (theta = startAngle; theta < endAngle;theta += deltaTheta)

019. {

020. a = (int)(X[i] - r*cos(theta) + 0.5);

021. b = (int)(Y[i] - r*sin(theta) + 0.5);

022. if (a > maxA)

023. {

024. maxA = a;

025. }else if (a < minA)

026. {

027. minA = a;

028. }

029.  

030. if (b > maxB)

031. {

032. maxB = b;

033. }else if (b < minB)

034. {

035. minB = b;

036. }

037.  

038. }

039. }

040. //确定a，b的范围之后，即确定了票箱的大小

041. int aScale = maxA - minA + 1;

042. int bScale = maxB - minB + 1;

043.  

044. int *VoteBox = new int[aScale*bScale];

045. //VoteBox初始化为0

046. for (int i = 0; i < aScale*bScale;i++)

047. {

048. VoteBox[i] = 0;

049. }

050. //开始投票

051. for (int i = 0; i < Cnt;i++)

052. {

053. //printf("%d  ",i);

054. for (theta = startAngle; theta < endAngle;theta += deltaTheta)

055. {

056.  

057. a = (int)(X[i] - r*cos(theta) + 0.5);

058. b = (int)(Y[i] - r*sin(theta) + 0.5);  

059. VoteBox[(b - minB)*aScale + a - minA] = VoteBox[(b - minB)*aScale + a - minA] + 1;

060. }

061. }

062.  

063. //筛选票箱

064. int VoteMax = 0;

065. int VoteMaxX,VoteMaxY;

066. for (int i = 0; i < bScale ;i++)

067. {

068. for (int j = 0; j < aScale ;j++)

069. {

070. if (VoteBox[i*aScale + j] > VoteMax)

071. {

072. VoteMax = VoteBox[i*aScale + j];

073. VoteMaxY = i;

074. VoteMaxX = j;

075. }

076. }

077. }

078.  

079. int Count = 0;

080. printf("VoteMax: %d",VoteMax);

081. for (int i = 0; i < bScale ;i++)

082. {

083. for (int j = 0; j < aScale ;j++)

084. {

085. if (VoteBox[i*aScale + j] >= VoteMax)

086. {

087. Count++;

088. }

089. }

090. }

091. printf("   %d \n",Count);

092. //释放内存

093. delete [] VoteBox;

094. if (VoteMax > 3)

095. {

096. Arc->center.x = VoteMaxX + minA;

097. Arc->center.y = VoteMaxY + minB;

098. Arc->r = r;

099. return 1;

100. }else {

101. return 0;

102. }

103. return 1;

104. }

2、最小二乘法拟合

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

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

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