Shape_detect:

	Erase old version methods (bool fit_arc and Point rotate) and
	constants (Arc_max_aperture, Arc_min_aperture and Arc_std_max) arc detection.

diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc
index dda8d3ed705caf987963ae74126bd756a88315e0..b4a720f86d597b61815b110a0d71119d4372e7b9 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.cc
+++ b/src/hokuyo/shape-detect/shape_detect.cc
@@ -19,10 +19,6 @@ Shape_detect::Shape_detect()
        Shape_detect::Line_min_points = 7;
        Shape_detect::Line_error_threshold = 20;
        Shape_detect::Max_distance_point = 300;
-
-       Shape_detect::Arc_std_max = 0.2;//0.15
-       Shape_detect::Arc_min_aperture = 1.57; //90 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)
@@ -42,125 +38,11 @@ inline Shape_detect::Point Shape_detect::intersection_line(const Shape_detect::P
        return tmp;
 }
 
-inline Shape_detect::Point Shape_detect::rotate(Shape_detect::Point input_point, float rad)
-{
-       Shape_detect::Point tmp;
-       tmp.x = input_point.x * cos(rad) - input_point.y * sin(rad);
-       tmp.y = input_point.x * sin(rad) + input_point.y * cos(rad);
-
-       return tmp;
-}
-
 inline float Shape_detect::point_distance(Shape_detect::Point a, Shape_detect::Point b)
 {
        return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
 }
 
-bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &arcs)
-{
-       //std::cout << "rozsah: " << begin << " - " << end << " -> ";
-
-       if (end-begin < 4) {
-               //std::cout << "mensi nez 4" << std::endl;
-               return false;
-       }
-
-       Point rotated_point = cartes[(end + begin) / 2];
-       Point right = cartes[begin];
-       Point left = cartes[end];
-       rotated_point.x = rotated_point.x - left.x;
-       rotated_point.y = rotated_point.y - left.y;
-       float angle_to_rotate = atan2((left.x - right.x) / (left.y - right.y),1);
-       rotated_point = rotate(rotated_point, angle_to_rotate);
-
-       //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)) {
-               //std::cout << "rotacni bod" << "; ";
-               return false;
-       }
-
-       int segment_size = end - begin + 1;
-       int slopes_size = segment_size - 3;
-
-       float ma, mb, slopes[slopes_size];
-
-       for (int i = 0; i < slopes_size; i++) {
-               Shape_detect::Point tmp = cartes[begin + i + 1];
-               ma = atan2(left.y - tmp.y, left.x - tmp.x);
-               mb = atan2(right.y - tmp.y, right.x - tmp.x);
-
-               slopes[i] = ma - mb;
-       }
-
-       float sum = 0;
-       for (int i = 0; i < slopes_size; i++)
-               sum = sum + slopes[i];
-
-       float average = sum / slopes_size;
-
-       float totalvar = 0;
-       int count = slopes_size;
-       
-       while (count > 0) {
-               count--;
-               totalvar = totalvar + ((slopes[count] - average) * (slopes[count] - average));
-       }
-
-       float standard_deviation = sqrt(totalvar / --slopes_size);
-
-       //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);
-
-               Shape_detect::Point center;
-               center.x = middle;
-               center.y = height;
-
-               center = Shape_detect::rotate(center, angle_to_rotate);
-               center.x = center.x + left.x;
-               center.y = center.y + left.y;
-
-               Shape_detect::Arc arc;
-               arc.center = center;
-               arc.begin = cartes[begin];
-               arc.end = cartes[end];
-
-               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 << "center: " << center.x << "; " << center.y << " >> ";
-
-               //std::cout << "Detekovan" << std::endl;
-               return 1;
-       }
-
-       //fit_arc(begin + 1, end - 1, arcs);
-
-       //std::cout << "Nic" << std::endl;
-
-       return 0;
-}
-
 void Shape_detect::bresenham_circle(const float &radius, const float &scale, std::vector<Shape_detect::Point> &circle)
 {
        int d;
@@ -311,6 +193,7 @@ void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arc
 */
        Shape_detect::Point center;
 
+
        arc.radius = radius; //radius [mm]
 
        int max = 0;
@@ -527,7 +410,6 @@ void Shape_detect::arc_detect(std::vector<Shape_detect::Arc> &arcs)
                end --;
 
                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]
                }
 

diff --git a/src/hokuyo/shape-detect/shape_detect.h b/src/hokuyo/shape-detect/shape_detect.h
index 7045486ad8e2e43cf8c757e71255e9424763939b..422af40e8b9122dc2fd5a3e105a5e804612e887b 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.h
+++ b/src/hokuyo/shape-detect/shape_detect.h
@@ -208,10 +208,6 @@ class Shape_detect
 
                std::vector<Point> cartes;//(HOKUYO_ARRAY_SIZE);
 
-               float Arc_max_aperture;
-               float Arc_min_aperture;
-               float Arc_std_max;
-
                /**
                 * Calculation of lines intersection.
                 * @param point which pertains to line.
@@ -260,10 +256,6 @@ class Shape_detect
                 */
                inline float point_distance(Point a, Point b);
 
-               inline Point rotate(Point input_point, float rad);
-
-               bool fit_arc(int begin, int end, std::vector<Arc> &arcs);
-
                void bresenham_circle(const float &radius,const float &scale, std::vector<Point> &circle);
 
                // radius [mm], scale [mm]