From e45eeaa282fc58c42bcbd23e66dd1c8532a54553 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Wed, 20 Apr 2011 22:32:00 +0200 Subject: [PATCH] shapedet: Draw hough's acumulator in gnuplot This is a quick hack, but it should be extremely useful in settings right parameters of haugh transform (scale and maybe some maximal distance or minimum number of points). In production code, the debugging should be turned off! This patch also converts all constants to millimeters, because I found it confusing to think about which variable is in which units. --- src/hokuyo/shape-detect/offline.cc | 21 ++++++++ src/hokuyo/shape-detect/shape_detect.cc | 71 +++++++++++++------------ src/hokuyo/shape-detect/shape_detect.h | 10 +++- 3 files changed, 67 insertions(+), 35 deletions(-) diff --git a/src/hokuyo/shape-detect/offline.cc b/src/hokuyo/shape-detect/offline.cc index 3b77b527..32c54905 100644 --- a/src/hokuyo/shape-detect/offline.cc +++ b/src/hokuyo/shape-detect/offline.cc @@ -55,8 +55,15 @@ void gnuplot(std::vector &cartes, fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n"); fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n"); fprintf(gnuplot, "set style fill transparent solid 0.2 noborder\n"); + fprintf(gnuplot, "set palette model RGB functions .8-gray*2, .8-gray*2, .8-gray*2\n"); fprintf(gnuplot, "plot"); + for (unsigned i=0; i < arcs.size(); i++) { + if (arcs[i].debug) + fprintf(gnuplot, "'-' matrix using ($1*%g+%g):($2*%g+%g):3 with image, ", + arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.x, + arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.y); + } if (cartes.size() > 0) fprintf(gnuplot, "'-' with points ls 1"); if (lines.size() > 0) @@ -65,6 +72,19 @@ void gnuplot(std::vector &cartes, fprintf(gnuplot, ", '-' with circles ls 2"); fprintf(gnuplot, "\n"); + // Draw accumulators + for (int i = 0; i < (int) arcs.size(); i++) { + Shape_detect::Arc &a = arcs[i]; + if (!a.debug) + continue; + for (int y=0; y < a.debug->acc_size; y++) { + for (int x=0; x < a.debug->acc_size; x++) { + fprintf(gnuplot, "%d ", a.debug->bitmap[y*a.debug->acc_size+x]); + } + fprintf(gnuplot, "\n"); + } + fprintf(gnuplot, "e\ne\n"); + } if (cartes.size() > 0) { for (int i = 0; i < (int) cartes.size(); i++) { @@ -81,6 +101,7 @@ void gnuplot(std::vector &cartes, fprintf(gnuplot, "e\n"); } if (arcs.size() > 0) { + // Draw circles for (int i = 0; i < (int) arcs.size(); i++) { fprintf(gnuplot, "%f %f %f\n", arcs[i].center.x, arcs[i].center.y, arcs[i].radius); diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc index 04c61864..00679816 100644 --- a/src/hokuyo/shape-detect/shape_detect.cc +++ b/src/hokuyo/shape-detect/shape_detect.cc @@ -161,22 +161,13 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector &a return 0; } -void Shape_detect::hough_transform_arc(int begin, int end, std::vector &arcs, float radius, int scale) +void Shape_detect::hough_transform_arc(int begin, int end, std::vector &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 = 200.0; - min_x = 0.0; - max_y = 200.0; - min_y = 0.0; - -/* max_x = cartes[begin].x; min_x = max_x; @@ -195,18 +186,28 @@ void Shape_detect::hough_transform_arc(int begin, int end, std::vector &arc else if (cartes[i].y < min_y) min_y = cartes[i].y; } + const float center_x = (max_x - min_x)/2 + min_x; + const float center_y = (max_y - min_y)/2 + min_y; - int distance_x = ceil(max_x++ - min_x--); - int distance_y = ceil(max_y++ - min_y--); -*/ - int distance_x = ceil(max_x / scale); - int distance_y = ceil(max_y / scale); + 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]; - int accumulator[distance_y][distance_x]; + memset(accumulator, 0, sizeof(accumulator)); - for (int i = 0; i < distance_y; i++) - for (int j = 0; j < distance_x; j++) - accumulator[i][j] = 0; // transformace int a, b; double phi; @@ -214,39 +215,41 @@ void Shape_detect::hough_transform_arc(int begin, int end, std::vector &arc double step = (2 * M_PI) / 180; for (int i = begin; i <= end; i++) { - int x_k = abs(floor(cartes[i].x / (10 * scale))); - int y_k = abs(floor(cartes[i].y / (10 * scale))); - + 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 = x_k - (radius / scale) * cos(phi); - b = y_k - (radius / scale) * sin(phi); + a = amid + x_k - (radius / scale) * cos(phi) ; + b = amid + y_k - (radius / scale) * sin(phi); - if (a > 0 && a < distance_x && b > 0 && b < distance_y) + 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::Arc arc; Shape_detect::Point center; - arc.radius = radius * 10; //radius [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]; - center.x = j * 10 * scale * signum_x; - center.y = i * 10 * scale * signum_y; + center.x = (j-amid) * scale + center_x; + center.y = (i-amid) * scale + center_y; } } } - if (max > 7 ) { + if (max > (end - begin) / 3 ) { arc.center = center; + memcpy(arc.debug->bitmap, accumulator, sizeof(accumulator)); arcs.push_back(arc); } @@ -441,7 +444,7 @@ void Shape_detect::arc_detect(std::vector &arcs) end --; //fit_arc(start, end, arcs); - hough_transform_arc(start, end, arcs, 10.0, 2); // radius [cm], scale[cm] + hough_transform_arc(start, end, arcs, 100.0, 10); // radius [mm], scale[mm] start = end + 1; } } diff --git a/src/hokuyo/shape-detect/shape_detect.h b/src/hokuyo/shape-detect/shape_detect.h index 6f4beaca..2ca2276a 100644 --- a/src/hokuyo/shape-detect/shape_detect.h +++ b/src/hokuyo/shape-detect/shape_detect.h @@ -141,6 +141,13 @@ class Shape_detect Point b; /**< end point from a line. */ } Line; + struct arc_debug { + Point acc_origin; + float acc_scale; + int acc_size; + int *bitmap; + }; + /** * Arc defined by TODO. * @ingroup shapedet @@ -150,6 +157,7 @@ class Shape_detect Point begin; Point end; float radius; + struct arc_debug *debug; } Arc; /** @@ -256,7 +264,7 @@ class Shape_detect bool fit_arc(int begin, int end, std::vector &arcs); - void hough_transform_arc(int begin, int end, std::vector &arcs, float radius, int scale); + void hough_transform_arc(int begin, int end, std::vector &arcs, float radius, float scale); }; #endif // SHAPE_DETECT -- 2.39.2