Laser Radar Learning Notes (V) Arc _ Laser Use

Source: Http://write.blog.csdn.net/postlist

In addition to the line, in the LIDAR data, the most important is the arc, the position of the arc to determine that the native does not have a straight line of high precision,

Therefore, it is not suitable for positioning of the benchmark, but the robot in the execution of the action, need to determine the position of the arc, or according to the arc to determine the target is

What or where the target is.

The detection of arcs includes the position of arcs (x,y) and the size R, commonly used methods include Hough transformation and least squares fitting.
The accuracy of the general arc position detection is low, which can not be used as the standard of positioning, but the position between the robot and the target is determined.

1. Hough Transform

Before the arc detection, we need to segment the data, divide a series of points into different regions, and then compute the position of the arc. Hough Transform

Do not need to know whether a region has a circular arc, similar to the voting mechanism, a parameter to obtain more votes, there is the possibility of arc, www.it165.net

is greater than a threshold value, you can assume that there is an arc in place.

X-a = R*cos (theta)

Y-b = R*sin (theta)

For each point x y, there are countless points that satisfy the formula, that is, there are countless circles at that point, each circle corresponding to a set of (a,b,r) parameters, set up a

Ballot box, there are possible (a,b,r) parameters, when a group of parameters appear once, it will be the ticket number in the ballot boxes plus 1, all the points are scanned

, look at the ballot boxes, the largest number of votes is the circle of the most probability of occurrence. A threshold should be set at this time, if the maximum number of votes is less than the threshold value,

It is believed that there is no circle, otherwise there is a circle.

This is a simplified Hough circle algorithm, assuming that the radius is known

  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;//interval 1 degrees 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. Calculates the minimum and maximum value of 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. After determining the range of a,b, the size of the ballot boxes is determined to 041. int Ascale = Maxa-mina +1; 042. int bscale = maxb-minb + 1; 043.   044. int *votebox = new Int[ascale*bscale]; 045.//votebox initialized to 0 046. for (int i = 0; i < ascale*bscale;i++) 047. {048. Votebox[i] = 0; 049.} 050. Start voting 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. Filter the ballot boxes 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.//Free memory 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; }else {0; 102.} 103. return 1; 104.}

2, least squares fitting

The least squares fitting is to determine whether it is a circular arc, arc can be fitted, otherwise the result of fitting is definitely not allowed.

But the judgment of the line can be judged by the method of polygon fitting, looking at the point and the distance of the line, and the arc is not

Very good judgment method, at least I have not found at present.