shape_detect: optimalization detection of obstacle (diameter 20cm).

[eurobot/public.git] / src / hokuyo / shape-detect / shape_detect.cc

diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc
index 6d950805ef52f15a6c17f7d6df998e9252a559ed..a670dfa522102ac80331bcb7eb4b5c52a6aa6935 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.cc
+++ b/src/hokuyo/shape-detect/shape_detect.cc
@@ -80,9 +80,9 @@ Shape_detect::Shape_detect()
        Shape_detect::Line_error_threshold = 20;
        Shape_detect::Max_distance_point = 300;
 
-       Shape_detect::Arc_std_max = 0.15; //0.15
+       Shape_detect::Arc_std_max = 0.2;//0.15
        Shape_detect::Arc_min_aperture = 1.57; //90 degrees
-       Shape_detect::Arc_max_aperture = 2.5; //2.365 #135 degrees
+       Shape_detect::Arc_max_aperture = 2.365; //2.365 #135 degrees
 }
 
 Shape_detect::Shape_detect(int line_min_points, int line_error_threshold, int max_distance_point)
@@ -135,18 +135,9 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
 
        //std::cout << "rotacni bod: " << rotated_point.x << ", " << rotated_point.y << "; vzdalenost: " << Shape_detect::point_distance(left, right) << "; ";
 
-       if (-rotated_point.x > .1 * Shape_detect::point_distance(left, right) &&
-               -rotated_point.x < Shape_detect::point_distance(left, right)) {
-/*
-               if (end - begin > 20) {
-                       end--;
-                       begin++;
-                       right = cartes[begin];
-                       left = cartes[end];
-               }
-*/
-       } else {
-               //std::cout << "rotace bodu" << std::endl;
+       if (-rotated_point.x < .1 * Shape_detect::point_distance(left, right) &&
+               -rotated_point.x > Shape_detect::point_distance(left, right)) {
+               //std::cout << "rotacni bod" << "; ";
                return false;
        }
 
@@ -179,17 +170,17 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
 
        float standard_deviation = sqrt(totalvar / --slopes_size);
 
-       //std::cout << "std: " << standard_deviation << ", Average: " << average << " => ";
+       //std::cout << "std: " << standard_deviation << ", Average: " << average << ": ";
 
        if (standard_deviation < Arc_std_max && Arc_max_aperture > abs(average) && abs(average) > Arc_min_aperture) {
                Shape_detect::Point tmp;
                tmp.x = right.x - left.x;
                tmp.y = right.y - left.y;
-               
+
                angle_to_rotate = atan2(tmp.y, tmp.x);
 
                tmp = Shape_detect::rotate(tmp, -angle_to_rotate);
-               
+
                float middle = tmp.x / 2.0;
                float height = middle * tan(average - M_PI / 2.0);
 
@@ -205,20 +196,28 @@ bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &a
                arc.center = center;
                arc.begin = cartes[begin];
                arc.end = cartes[end];
-               arc.radius = float(sqrt((left.x - center.x)*(left.x - center.x)+(left.y - center.y)*(left.y - center.y)));
-               //arc.radius = float(sqrt(center.y * center.y + center.x * center.x));
+
+               arc.radius = 0;
+               for (int i = 0; i < segment_size; i++) {
+                       tmp.x = cartes[begin + i].x;
+                       tmp.y = cartes[begin + i].y;
+                       arc.radius = arc.radius + float(sqrt((tmp.x - center.x)*(tmp.x - center.x)+(tmp.y - center.y)*(tmp.y - center.y)));
+               }
+
+               arc.radius = arc.radius / segment_size;
 
                arcs.push_back(arc);
 
-               //std::cout << std::endl << std::endl << "tmp: " << tmp.x << " , " << tmp.y << "; center: " << center.x << " , " << center.y << std::endl;
+               //std::cout << "center: " << center.x << "; " << center.y << " >> ";
 
                //std::cout << "Detekovan" << std::endl;
                return 1;
        }
 
-       fit_arc(begin + 1, end - 1, arcs);
+       //fit_arc(begin + 1, end - 1, arcs);
 
        //std::cout << "Nic" << std::endl;
+
        return 0;
 }