return 0;
}
- void Shape_detect::bresenham_circle(int scale_radius, std::vector<Shape_detect::Point> &circle)
++void Shape_detect::bresenham_circle(const float &radius, const float &scale, std::vector<Shape_detect::Point> &circle)
+{
+ int d;
+
++ int scale_radius = abs((radius / 10) / (scale / 10));
++
+ int x = 0;
+ int y = scale_radius;
+
+ Shape_detect::Point tmp;
+
+ tmp.x = x;
+ tmp.y = y;
+
+ circle.push_back(tmp);
+
+ d = 3 - (2 * scale_radius);
+
+ for (x = 0; x <= y; x++) {
+ if (d < 0) {
+ y = y;
+ d = (d + (4 * x) + 6);
+ } else {
+ y--;
+ d = d + (4 * (x - y) + 10);
+ }
+
+ tmp.x = x;
+ tmp.y = y;
+
+ circle.push_back(tmp);
+ }
-
++
+ return;
+}
+
- void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arcs, int radius, int scale)
+ void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arcs, float radius, float scale)
{
if ((end - begin) < 10)
return;
- int signum_x = (cartes[end].x < 0) ? -1 : 1;
- int signum_y = (cartes[end].y < 0) ? -1 : 1;
+ float max_x, max_y, min_x, min_y;
+
+ max_x = cartes[begin].x;
+ min_x = max_x;
- int max_x, max_y, min_x, min_y; // cm
+ max_y = cartes[begin].y;
+ min_y = max_y;
- max_x = 300;
- min_x = 0;
- max_y = 210;
- min_y = 0;
+ for (int i = begin; i <= end; i++) {
+ //std::cout << cartes[i].x << " -- " << cartes[i].y << " || ";
+ if (cartes[i].x > max_x)
+ max_x = cartes[i].x;
+ else if (cartes[i].x < min_x)
+ min_x = cartes[i].x;
-
++
+ if (cartes[i].y > max_y)
+ max_y = cartes[i].y;
+ else if (cartes[i].y < min_y)
+ min_y = cartes[i].y;
+ }
+
- int distance_x = ceil(max_x / scale);
- int distance_y = ceil(max_y / scale);
+ const float center_x = (max_x - min_x)/2 + min_x;
+ const float center_y = (max_y - min_y)/2 + min_y;
- int accumulator[distance_y][distance_x];
- const int asize = 600/scale;
- const int amid = asize/2;
++ const int asize = 600 / scale;
++ const int amid = asize / 2;
+
+ Shape_detect::Arc arc;
+ //arc.debug = 0;
+ arc.debug = new (struct arc_debug);
+
+ if (arc.debug) {
+ arc.debug->bitmap = new int[asize*asize];
+ arc.debug->acc_size = asize;
+ arc.debug->acc_origin.x = center_x - amid*scale;
+ arc.debug->acc_origin.y = center_y - amid*scale;
+ arc.debug->acc_scale = scale;
+ }
+
+ int accumulator[asize][asize];
memset(accumulator, 0, sizeof(accumulator));
-// transformace
+ std::vector<Shape_detect::Point> circle;
+
- bresenham_circle(radius / scale, circle);
-
++ bresenham_circle(radius, scale, circle);
+
+ for (int i = begin; i <= end; i++) {
- int xc = abs(floor(cartes[i].x / (10 * scale)));
- int yc = abs(floor(cartes[i].y / (10 * scale)));
++ int xc = floor((cartes[i].x - center_x) / scale);
++ int yc = floor((cartes[i].y - center_y) / scale);
+
+ for (unsigned int i = 0; i < circle.size(); i++) {
- accumulator[yc + (int) circle[i].x][xc + (int) circle[i].y] += 1;
- accumulator[yc - (int) circle[i].x][xc - (int) circle[i].y] += 1;
- accumulator[yc - (int) circle[i].x][xc + (int) circle[i].y] += 1;
- accumulator[yc + (int) circle[i].x][xc - (int) circle[i].y] += 1;
- accumulator[yc + (int) circle[i].y][xc + (int) circle[i].x] += 1;
- accumulator[yc - (int) circle[i].y][xc - (int) circle[i].x] += 1;
- accumulator[yc - (int) circle[i].y][xc + (int) circle[i].x] += 1;
- accumulator[yc + (int) circle[i].y][xc - (int) circle[i].x] += 1;
++ int par[8];
++
++ par[0] = amid + yc + (int) circle[i].x;
++ par[1] = amid + yc - (int) circle[i].x;
++ par[2] = amid + xc + (int) circle[i].x;
++ par[3] = amid + xc - (int) circle[i].x;
++ par[4] = amid + yc + (int) circle[i].y;
++ par[5] = amid + yc - (int) circle[i].y;
++ par[6] = amid + xc + (int) circle[i].y;
++ par[7] = amid + xc - (int) circle[i].y;
++
++ if (par[0] > 0 && par[0] < asize && par[6] > 0 && par[6] < asize)
++ accumulator[par[0]][par[6]] += 1;
++
++ if (par[1] > 0 && par[1] < asize && par[7] > 0 && par[7] < asize)
++ accumulator[par[1]][par[7]] += 1;
++
++ if (par[1] > 0 && par[1] < asize && par[6] > 0 && par[6] < asize)
++ accumulator[par[1]][par[6]] += 1;
++
++ if (par[0] > 0 && par[0] < asize && par[7] > 0 && par[7] < asize)
++ accumulator[par[0]][par[7]] += 1;
++
++ if (par[4] > 0 && par[4] < asize && par[2] > 0 && par[2] < asize)
++ accumulator[par[4]][par[2]] += 1;
++
++ if (par[5] > 0 && par[5] < asize && par[3] > 0 && par[3] < asize)
++ accumulator[par[5]][par[3]] += 1;
++
++ if (par[5] > 0 && par[5] < asize && par[2] > 0 && par[2] < asize)
++ accumulator[par[5]][par[2]] += 1;
++
++ if (par[4] > 0 && par[4] < asize && par[3] > 0 && par[3] < asize)
++ accumulator[par[4]][par[3]] += 1;
+ }
+ }
+
- Shape_detect::Arc arc;
++/*
+ int a, b;
+ double phi;
+
+ double step = (2 * M_PI) / 180;
+
+ for (int i = begin; i <= end; i++) {
+ int x_k = floor((cartes[i].x - center_x) / scale);
+ int y_k = floor((cartes[i].y - center_y) / scale);
+ int last_a = 0, last_b = 0;
+ for (phi = 0.0; phi < 2 * M_PI; phi += step) {
+ a = amid + x_k - (radius / scale) * cos(phi) ;
+ b = amid + y_k - (radius / scale) * sin(phi);
+
+ if (a > 0 && a < asize && b > 0 && b < asize && (a != last_a || b != last_b))
+ accumulator[b][a] += 1;
+ last_a = a;
+ last_b = b;
+ }
+ }
-
++*/
Shape_detect::Point center;
- arc.radius = radius * 10; //radius v arc [mm]!!!
+ arc.radius = radius; //radius [mm]
- int max = 1;
+ int max = 0;
- for (int i = 0; i < distance_y; i++) {
- for (int j = 0; j < distance_x; j++) {
+ for (int i = 0; i < asize; i++) {
+ for (int j = 0; j < asize; j++) {
if (accumulator[i][j] > max) {
max = accumulator[i][j];
arcs.push_back(arc);
}
--// tisk
-/*
- std::cout << "Tisk" << std::flush << std::endl;
--
-- for (int i = 0; i < distance_y; i++) {
-- for (int j = 0; j < distance_x; j++) {
- std::cout << accumulator[i][j] << " ";
- std::cout << accumulator[i][j];
-- }
-- std::cout << std::endl;
-- }
-
-*/
return;
}
end --;
//fit_arc(start, end, arcs);
- hough_transform_arc(start, end, arcs, 10, 2);
++
+ hough_transform_arc(start, end, arcs, 100.0, 10); // radius [mm], scale[mm]
++
start = end + 1;
}
}
projects / eurobot / public.git / commitdiff