Update shape-detect

indentation fixes + algorithm changes

diff --git a/src/hokuyo/shape-detect/main.cc b/src/hokuyo/shape-detect/main.cc
index 4260c5ef2a48034484c3ec002c0d765d47eae4bf..bdfd2a13722d31f356f8dd2a0754a458e6d1fd6b 100644 (file)
--- a/src/hokuyo/shape-detect/main.cc
+++ b/src/hokuyo/shape-detect/main.cc
@@ -5,11 +5,8 @@
 #include <sstream>
 #include <math.h>
 #include <vector>
-
-#define PI 3.14159265
-
-// resolution laser (step)
-#define RESOLUTION 0.331
+#include <hokuyo.h>
+#include <robomath.h>
 
 #define LINE_MIN_POINTS 7
 #define LINE_ERROR_THRESHOLD 25
@@ -21,16 +18,17 @@ typedef struct {float x,y;} Point;
 vector<Point> cartes;
 vector<Point> 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<int> &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<int> &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