From: Michal Sojka Date: Wed, 8 Dec 2010 09:43:12 +0000 (+0100) Subject: Update shape-detect X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/362b8f52adeb8e663fbc4bc76fa2aee428b4f306 Update shape-detect indentation fixes + algorithm changes --- diff --git a/src/hokuyo/shape-detect/main.cc b/src/hokuyo/shape-detect/main.cc index 4260c5ef..bdfd2a13 100644 --- a/src/hokuyo/shape-detect/main.cc +++ b/src/hokuyo/shape-detect/main.cc @@ -5,11 +5,8 @@ #include #include #include - -#define PI 3.14159265 - -// resolution laser (step) -#define RESOLUTION 0.331 +#include +#include #define LINE_MIN_POINTS 7 #define LINE_ERROR_THRESHOLD 25 @@ -21,16 +18,17 @@ typedef struct {float x,y;} Point; vector cartes; vector lines; -int perpendicular_regression(float &r, const int begin, const int end) { - int number_points = abs(end-begin) + 1; +int perpendicular_regression(float &r, int &max_diff_idx, const int begin, const int end) +{ + int number_points = abs(end-begin) + 1; if (number_points <= 0) return 1; - float sum_x = 0; - float sum_y = 0; + float sum_x = 0; + float sum_y = 0; - for (int i = begin; i <= end; i++) { - sum_x = sum_x + cartes[i].x; + for (int i = begin; i <= end; i++) { + sum_x = sum_x + cartes[i].x; sum_y = sum_y + cartes[i].y; } @@ -68,39 +66,49 @@ int perpendicular_regression(float &r, const int begin, const int end) { // maximum error r = 0; + unsigned ir = -1; float dist; for (int i = begin; i < end; i++) { // distance point from the line (A = m1, B = -1, C = b1) dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) ); - if (dist > r) r = dist; + if (dist > r) { + r = dist; + ir = i; + } } - float r1 = r; + float r1 = 0; + unsigned ir1 = -1; - // maximum error - r = 0; - for (int i = begin; i < end; i++) { // distance point from the line (A = m2, B = -1, C = b2) dist = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) ); - if (dist > r) r = dist; + if (dist > r1) { + r1 = dist; + ir1 = i; + } } - r = (r1 > r) ? r : r1; + if (r < r1) + max_diff_idx = ir; + else { + r = r1; + max_diff_idx = ir1; + } return 0; - } // line recursive fitting void line_fitting(int begin, int end) { + int line_break_point; cout << "begin: " << begin << " end: " << end << endl << flush; if ((end - begin) < LINE_MIN_POINTS) return; float r; - if (perpendicular_regression(r, begin, end)) return; // r = 0 + if (perpendicular_regression(r, line_break_point, begin, end)) return; // r = 0 if (r < LINE_ERROR_THRESHOLD) { lines.push_back(cartes[begin]); @@ -109,39 +117,12 @@ void line_fitting(int begin, int end) { cout << endl << "begin X: " << cartes[begin].x << " Y: " << cartes[begin].y << endl << flush; cout << "end X: " << cartes[end].x << " Y: " << cartes[end].y << endl << endl << flush; } else { - // Ax+By+C=0 - // normal vector: n[n_x, -n_y] - float n_x = cartes[begin].y - cartes[end].y; - float n_y = cartes[begin].x - cartes[end].x; - - float A = n_x; - float B = -n_y; - float C = n_y*cartes[end].y - n_x*cartes[end].x; - - int line_break_point = 0; - float dist, dist_max = 0; - - for (int i = begin; i < end; i++) { - // distance point from the line - dist = fabs( (cartes[i].x*A + cartes[i].y*B + C) / sqrt(A*A + B*B)); - - if (dist > dist_max) { - dist_max = dist; - line_break_point = i; - } - } - - if (dist_max > LINE_ERROR_THRESHOLD) { - line_fitting(begin, line_break_point); - line_fitting(line_break_point, end); - } else { - - lines.push_back(cartes[begin]); - lines.push_back(cartes[end]); - cout << endl << "begin X: " << cartes[begin].x << " Y: " << cartes[begin].y << endl << flush; - cout << "end X: " << cartes[end].x << " Y: " << cartes[end].y << endl << endl << flush; - - } + if (line_break_point == begin) + line_break_point++; + else if (line_break_point == end) + line_break_point--; + line_fitting(begin, line_break_point); + line_fitting(line_break_point, end); } // end if (r <= LINE_ERROR_THRESHOLD) return; @@ -151,32 +132,39 @@ void line_fitting(int begin, int end) { // polar to cartesian coordinates void polar_to_cartes(const vector &input_data) { - Point point; + Point point; - float fi; - int r; + float fi; + int r; - for (int i = 0; i < (int) input_data.size()-1; i += 2) { - r = (input_data[i+1] <= 19) ? 0 : input_data[i+1]; - fi = (RESOLUTION * input_data[i] - 45) * PI/180; + for (int i = 0; i < (int) input_data.size()-1; i += 2) { + r = (input_data[i+1] <= 19) ? 0 : input_data[i+1]; + //fi = HOKUYO_INDEX_TO_RAD(RESOLUTION * input_data[i] - 45) * PI/180; + if (r > 0) { + fi = HOKUYO_INDEX_TO_RAD(input_data[i]); - point.x = r * cos(fi); - point.y = r * sin(fi); - - point.x = (point.x == -0) ? 0 : point.x; - point.y = (point.y == -0) ? 0 : point.y; - - if (point.x != 0 && point.y != 0) cartes.push_back(point); - } + point.x = r * cos(fi); + point.y = r * sin(fi); + cartes.push_back(point); + } + } } void shape_detect(const vector &input_data) { // polar coordinates to cartesian coordinates - polar_to_cartes(input_data); + polar_to_cartes(input_data); cout << "Cartes vector size: " << cartes.size() << endl << endl; - - // detection lines - line_fitting(0, (int) cartes.size()-1); + + int start = 0; + while (..) { + i = start + 1; + while (distance(cartes[i-1], cartes[i]) < 50cm && + i < cartes.size()) + i++; + + // detection lines + line_fitting(start, i); + } } // MAIN