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

出处：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 sourceprint?

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、最小二乘法拟合

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

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

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