Shape_detect: add method hough_transform_arc.

diff --git a/src/hokuyo/shape-detect/offline.cc b/src/hokuyo/shape-detect/offline.cc
index ce4804fc9f3629884371b3a9c38a70af462ef04d..3b77b527784414cbeb5bd35def7aaa29d77018f7 100644 (file)
--- a/src/hokuyo/shape-detect/offline.cc
+++ b/src/hokuyo/shape-detect/offline.cc
@@ -87,8 +87,10 @@ void gnuplot(std::vector<Shape_detect::Point> &cartes,
                }
                fprintf(gnuplot, "e\n");
        }
-
        
+       fflush(gnuplot);
+       getchar();
+
        pclose(gnuplot);
 }
 
@@ -153,8 +155,9 @@ int main(int argc, char** argv)
                cout << "Arc: " << arcs[i] << endl;
        }
 
-       if (plot)
+       if (plot) {
                gnuplot(sd.getCartes(), lines, arcs);
+       }
 
        return 0;
 }

diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc
index 3b2bc9abc2bf592e6d25dd69a68244d9f49c048d..04c61864a25bd42268beb5c6df21b24154bc45e8 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.cc
+++ b/src/hokuyo/shape-detect/shape_detect.cc
@@ -161,6 +161,109 @@ 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)
+{
+       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;
+
+       max_y = cartes[begin].y;
+       min_y = max_y;
+
+       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++ - min_x--);
+       int distance_y = ceil(max_y++ - min_y--);
+*/
+       int distance_x = ceil(max_x / scale);
+       int distance_y = ceil(max_y / scale);
+
+       int accumulator[distance_y][distance_x];
+
+       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;
+
+       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)));
+
+               for (phi = 0.0; phi < 2 * M_PI; phi += step) {
+                       a = x_k - (radius / scale) * cos(phi);
+                       b = y_k - (radius / scale) * sin(phi);
+
+                       if (a > 0 && a < distance_x && b > 0 && b < distance_y)
+                               accumulator[b][a] += 1;
+               }
+       }
+
+       Shape_detect::Arc arc;
+       Shape_detect::Point center;
+
+       arc.radius = radius * 10; //radius [mm]
+
+       int max = 1;
+
+       for (int i = 0; i < distance_y; i++) {
+               for (int j = 0; j < distance_x; j++) {
+                       if (accumulator[i][j] > max) {
+
+                               max = accumulator[i][j];
+
+                               center.x = j * 10 * scale * signum_x;
+                               center.y = i * 10 * scale * signum_y;
+                       }
+               }
+       }
+
+       if (max > 7 ) {
+               arc.center = center;
+               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 << std::endl;
+       }
+*/
+       return;
+}
+
 int Shape_detect::perpendicular_regression(float &r, const int begin, const int end, General_form &gen)
 {
        int number_points = abs(end-begin) + 1;
@@ -337,7 +440,8 @@ void Shape_detect::arc_detect(std::vector<Shape_detect::Arc> &arcs)
 
                end --;
 
-               fit_arc(start, end, arcs);
+               //fit_arc(start, end, arcs);
+               hough_transform_arc(start, end, arcs, 10.0, 2); // radius [cm], scale[cm]
                start = end + 1;
        }
 }

diff --git a/src/hokuyo/shape-detect/shape_detect.h b/src/hokuyo/shape-detect/shape_detect.h
index 81d2810ddb864d638e36fea33b97ed356e6911e5..6f4beaca5c7ee0ea434bb097b60e6fd23bfa9838 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.h
+++ b/src/hokuyo/shape-detect/shape_detect.h
@@ -255,6 +255,8 @@ class Shape_detect
                inline Point rotate(Point input_point, float rad);
 
                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);
 };
 
 #endif // SHAPE_DETECT