Shape_detect: Optimization of constant detection arc and filtration detected the...

diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc
index 71caf5d414c5732d712b179ae5075ced6093ad48..dda8d3ed705caf987963ae74126bd756a88315e0 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.cc
+++ b/src/hokuyo/shape-detect/shape_detect.cc
@@ -165,7 +165,7 @@ void Shape_detect::bresenham_circle(const float &radius, const float &scale, std
 {
        int d;
 
-       int scale_radius = abs((radius / 10) / (scale / 10));
+       int scale_radius = abs(radius / scale);
 
        int x = 0;
        int y = scale_radius;
@@ -199,9 +199,6 @@ void Shape_detect::bresenham_circle(const float &radius, const float &scale, std
 
 void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arcs, float radius, float scale)
 {
-       if ((end - begin) < 10)
-               return;
-
        float max_x, max_y, min_x, min_y;
 
        max_x = cartes[begin].x;
@@ -330,7 +327,12 @@ void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arc
                }
        }
 
-       if (max > (end - begin) / 3 ) {
+       Shape_detect::Point origin;
+
+       origin.x = 0.0;
+       origin.y = 0.0;
+
+       if (max > (end - begin) / 3 && point_distance(origin, center) > point_distance(origin, cartes[end])) {
                arc.center = center;
                memcpy(arc.debug->bitmap, accumulator, sizeof(accumulator));
                arcs.push_back(arc);
@@ -507,17 +509,27 @@ void Shape_detect::arc_detect(std::vector<Shape_detect::Arc> &arcs)
        int cartes_size = cartes.size();
 
        int end, start = 0;
+
+       int radius = 100.0;
+
        while (start < cartes_size) {
+               Shape_detect::Point tmp_start = cartes[start];
+
                end = start + 1;
                
-               while (point_distance(cartes[end-1], cartes[end]) < 50 && end < cartes_size)
+               while (point_distance(cartes[end-1], cartes[end]) < 100
+                       && end < cartes_size
+                       && cartes[end].x < 1000
+                       && cartes[end].y < 1000) {
                        end++;
+               }
 
                end --;
 
-               //fit_arc(start, end, arcs);
-
-               hough_transform_arc(start, end, arcs, 100.0, 10); // radius [mm], scale[mm]
+               if (point_distance(tmp_start, cartes[end]) < (2 * radius) && (end - start > 10)) {
+                       //fit_arc(start, end, arcs);
+                       hough_transform_arc(start, end, arcs, radius, 10); // radius [mm], scale[mm]
+               }
 
                start = end + 1;
        }