src/hokuyo/shape-detect/main.cc

   1 #include <string>
   2 #include <stdlib.h>
   3 #include <fstream>
   4 #include <iostream>
   5 #include <sstream>
   6 #include <math.h>
   7 #include <vector>
   8 #include <hokuyo.h>
   9 #include <robomath.h>
  10
  11 #define LINE_MIN_POINTS 7
  12 #define LINE_ERROR_THRESHOLD 25
  13
  14 using namespace std;
  15
  16 typedef struct {float x,y;} Point;
  17
  18 vector<Point> cartes;
  19 vector<Point> lines;
  20
  21 int perpendicular_regression(float &r, int &max_diff_idx, const int begin, const int end)
  22 {
  23         int number_points = abs(end-begin) + 1;
  24
  25         if (number_points <= 0) return 1;
  26
  27         float sum_x = 0;
  28         float sum_y = 0;
  29
  30         for (int i = begin; i <= end; i++) {
  31                 sum_x = sum_x + cartes[i].x;
  32                 sum_y = sum_y + cartes[i].y;
  33         }
  34
  35         float med_x = sum_x / number_points;
  36         float med_y = sum_y / number_points;
  37
  38         vector<Point> point_average;
  39
  40         Point tmp;
  41
  42         for (int i = begin; i <= end; i++) {
  43                 tmp.x = cartes[i].x - med_x;
  44                 tmp.y = cartes[i].y - med_y;
  45                 point_average.push_back(tmp);
  46         }
  47
  48         float A = 0;
  49         float sum_xy = 0;
  50
  51         for (int i = 0; i < number_points; i++) {
  52                 A = A + (point_average[i].x*point_average[i].x - point_average[i].y*point_average[i].y);
  53                 sum_xy = sum_xy + point_average[i].x * point_average[i].y;
  54         }
  55
  56         if (sum_xy == 0) sum_xy = 1e-8;
  57
  58         A = A / sum_xy;
  59
  60         // tan(q)^2 + A*tan(q) - 1 = 0 ( tan(q) sign as m ) -> quadratic equation
  61         float m1 = (-A + sqrt(A*A + 4)) / 2;
  62         float m2 = (-A + sqrt(A*A + 4)) / 2;
  63
  64         float b1 = med_y - m1*med_x;
  65         float b2 = med_y - m2*med_x;
  66
  67         // maximum error
  68         r = 0;
  69         unsigned ir = -1;
  70         float dist;
  71
  72         for (int i = begin; i < end; i++) {
  73                 // distance point from the line (A = m1, B = -1, C = b1)
  74                 dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) );
  75                 if (dist > r) {
  76                         r = dist;
  77                         ir = i;
  78                 }
  79         }
  80
  81         float r1 = 0;
  82         unsigned ir1 = -1;
  83
  84         for (int i = begin; i < end; i++) {
  85                 // distance point from the line (A = m2, B = -1, C = b2)
  86                 dist = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
  87                 if (dist > r1) {
  88                         r1 = dist;
  89                         ir1 = i;
  90                 }
  91         }
  92
  93         if (r < r1)
  94                 max_diff_idx = ir;
  95         else {
  96                 r = r1;
  97                 max_diff_idx = ir1;
  98         }
  99
 100         return 0;
 101 }
 102
 103 // line recursive fitting
 104 void line_fitting(int begin, int end) {
 105         int line_break_point;
 106         cout << "begin: " << begin << " end: " << end << endl << flush;
 107
 108         if ((end - begin) < LINE_MIN_POINTS) return;
 109
 110         float r;
 111         if (perpendicular_regression(r, line_break_point, begin, end)) return; // r = 0
 112
 113         if (r < LINE_ERROR_THRESHOLD) {
 114                 lines.push_back(cartes[begin]);
 115                 lines.push_back(cartes[end]);
 116
 117                 cout << endl << "begin X: " << cartes[begin].x << " Y: " << cartes[begin].y << endl << flush;
 118                 cout << "end X: " << cartes[end].x << " Y: " << cartes[end].y << endl << endl << flush;
 119         } else {
 120                 if (line_break_point == begin)
 121                         line_break_point++;
 122                 else if (line_break_point == end)
 123                         line_break_point--;
 124                 line_fitting(begin, line_break_point);
 125                 line_fitting(line_break_point, end);
 126         } // end if (r <= LINE_ERROR_THRESHOLD)
 127
 128         return;
 129
 130 }
 131
 132 // polar to cartesian coordinates
 133 void polar_to_cartes(const vector<int> &input_data) {
 134
 135         Point point;
 136
 137         float fi;
 138         int r;
 139
 140         for (int i = 0; i < (int) input_data.size()-1; i += 2) {
 141                 r = (input_data[i+1] <= 19) ? 0 : input_data[i+1];
 142                 //fi = HOKUYO_INDEX_TO_RAD(RESOLUTION * input_data[i] - 45) * PI/180;
 143                 if (r > 0) {
 144                         fi = HOKUYO_INDEX_TO_RAD(input_data[i]);
 145
 146                         point.x = r * cos(fi);
 147                         point.y = r * sin(fi);
 148                         cartes.push_back(point);
 149                 }
 150         }
 151 }
 152
 153 void shape_detect(const vector<int> &input_data) {
 154         // polar coordinates to cartesian coordinates
 155         polar_to_cartes(input_data);
 156         cout << "Cartes vector size: " << cartes.size() << endl << endl;
 157
 158         int start = 0;
 159         while (..) {
 160                 i = start + 1;
 161                 while (distance(cartes[i-1], cartes[i]) < 50cm &&
 162                        i < cartes.size())
 163                         i++;
 164
 165                 // detection lines
 166                 line_fitting(start, i);
 167                 start = i+1;
 168         }
 169 }
 170
 171 struct robottype_orte_data orte;
 172
 173 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 174                         void *recvCallBackParam)
 175 {
 176         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 177         struct hokuyo_scan_type scan;
 178
 179         switch (info->status) {
 180                 case NEW_DATA: {
 181                         scan = *instance;
 182                         scan.data[i]
 183                         shape_detect(scan);
 184                         save_data(lines);
 185                         system("gnuplot graph.dem"); // nebo pres pipe jako v barcam
 186                         break;
 187                 }
 188                 case DEADLINE:
 189                         robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
 190                         //system("killall -9 hokuyo");
 191                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 192                         break;
 193         }
 194 }
 195 int robot_init_orte()
 196 {
 197         int rv = 0;
 198
 199         robot.orte.strength = 20;
 200
 201         rv = robottype_roboorte_init(&orte);
 202         if (rv)
 203                 return rv;
 204
 205         robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
 206
 207         return rv;
 208 }
 209
 210
 211 // MAIN
 212 int main(int argc, char** argv) {
 213
 214   if (argc < 2) {
 215     cout << "Error: Invalid number of input parameters." << endl;
 216     return 1;
 217   }
 218
 219   vector<int> input_data;
 220
 221   ifstream infile (argv[1], ios_base::in);
 222
 223   string line;  // line input file
 224   int number;           // number from input file
 225
 226   while (getline(infile, line, ',')) {
 227     if (line != "\n") {
 228       stringstream strStream(line);
 229       strStream >> number;
 230       input_data.push_back(number);
 231     }
 232   }
 233
 234         // detect line
 235         shape_detect(input_data);
 236
 237   // save file
 238   FILE *file = fopen("output_data","w");
 239
 240   for (int i = 0; i < (int) lines.size(); i++)
 241     fprintf(file, "%f, %f\n", lines[i].x, lines[i].y);
 242
 243   fclose(file);
 244
 245   file = fopen("cartes_data","w");
 246
 247   for (int i = 0; i < (int) cartes.size(); i++)
 248     fprintf(file, "%f, %f\n", cartes[i].x, cartes[i].y);
 249
 250   fclose(file);
 251
 252   return 0;
 253 }
 254