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.

diff --git a/src/hokuyo/shape-detect/offline.cc b/src/hokuyo/shape-detect/offline.cc
index 3b77b527784414cbeb5bd35def7aaa29d77018f7..32c5490507526111ac5459edc69478511f503f9d 100644 (file)
--- a/src/hokuyo/shape-detect/offline.cc
+++ b/src/hokuyo/shape-detect/offline.cc
@@ -55,8 +55,15 @@ void gnuplot(std::vector<Shape_detect::Point> &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<Shape_detect::Point> &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<Shape_detect::Point> &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 04c61864a25bd42268beb5c6df21b24154bc45e8..006798167fd3f15ecbc704da8acf66a3a7fb86ad 100644 (file)
--- 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<Shape_detect::Arc> &a
        return 0;
 }
 
-void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arcs, float 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 = 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> &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> &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<Shape_detect::Arc> &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 6f4beaca5c7ee0ea434bb097b60e6fd23bfa9838..2ca2276abab0d9bb71074c22f83002f950b9d864 100644 (file)
--- 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<Arc> &arcs);
 
-               void hough_transform_arc(int begin, int end, std::vector<Arc> &arcs, float radius, int scale);
+               void hough_transform_arc(int begin, int end, std::vector<Arc> &arcs, float radius, float scale);
 };
 
 #endif // SHAPE_DETECT