src/hokuyo/shape-detect/main.cc

   1 #include <string>
   2 #include <cstring>
   3 #include <stdlib.h>
   4 #include <fstream>
   5 #include <iostream>
   6 #include <sstream>
   7 #include <math.h>
   8 #include <vector>
   9 #include <hokuyo.h>
  10 #include <robot.h>
  11 #include <robomath.h>
  12 #include <robottype.h>
  13 #include <roboorte_robottype.h>
  14
  15 #define LINE_MIN_POINTS 7
  16 #define LINE_ERROR_THRESHOLD 20
  17 #define MAX_DISTANCE_POINT 300
  18
  19 using namespace std;
  20
  21 typedef struct {float x,y;} Point;
  22
  23 typedef struct {Point a,b;} Line;
  24
  25 FILE *gnuplot;
  26
  27 int perpendicular_regression(float &r, int &max_diff_idx, const int begin, const int end, vector<Point> &cartes)
  28 {
  29         int number_points = abs(end-begin) + 1;
  30
  31         if (number_points <= 0) return 1;
  32
  33         float sum_x = 0;
  34         float sum_y = 0;
  35
  36         for (int i = begin; i <= end; i++) {
  37                 sum_x = sum_x + cartes[i].x;
  38                 sum_y = sum_y + cartes[i].y;
  39         }
  40
  41         float med_x = sum_x / number_points;
  42         float med_y = sum_y / number_points;
  43
  44         vector<Point> point_average(number_points);
  45
  46         Point tmp;
  47
  48         int j = 0;
  49
  50         for (int i = begin; i <= end; i++) {
  51                 tmp.x = cartes[i].x - med_x;
  52                 tmp.y = cartes[i].y - med_y;
  53                 point_average[j] = tmp;
  54                 j++;
  55         }
  56
  57         float A = 0;
  58         float sum_xy = 0;
  59
  60         for (int i = 0; i < number_points; i++) {
  61                 A = A + (point_average[i].x*point_average[i].x - point_average[i].y*point_average[i].y);
  62                 sum_xy = sum_xy + point_average[i].x * point_average[i].y;
  63         }
  64
  65         if (sum_xy == 0) sum_xy = 1e-8;
  66
  67         A = A / sum_xy;
  68
  69         // tan(q)^2 + A*tan(q) - 1 = 0 ( tan(q) sign as m ) -> quadratic equation
  70         float m1 = (-A + sqrt(A*A + 4)) / 2;
  71         float m2 = (-A - sqrt(A*A + 4)) / 2;
  72
  73         float b1 = med_y - m1*med_x;
  74         float b2 = med_y - m2*med_x;
  75
  76         // maximum error
  77         r = 0;
  78         unsigned ir = -1;
  79         float dist;
  80
  81         float r1 = 0;
  82         unsigned ir1 = -1;
  83         float dist1;
  84
  85         for (int i = begin; i < end; i++) {
  86                 // distance point from the line (A = m1, B = -1, C = b1)
  87                 dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) );
  88                 dist1 = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
  89
  90                 if (dist1 > r1) {
  91                         r1 = dist1;
  92                         ir1 = i;
  93                 }
  94
  95                 if (dist > r) {
  96                         r = dist;
  97                         ir = i;
  98                 }
  99         }
 100
 101         if (r < r1) {
 102                 max_diff_idx = ir;
 103         } else {
 104                 r = r1;
 105                 max_diff_idx = ir1;
 106         }
 107
 108         return 0;
 109 }
 110
 111 void plot_line(int begin, int end, int maxdist, vector<Point> &cartes, vector<Line> &lines)
 112 {
 113         fprintf(gnuplot, "set grid\n");
 114         fprintf(gnuplot, "set nokey\n");
 115         fprintf(gnuplot, "set style line 1 lt 2 lc rgb \"red\" lw 3\n");
 116         fprintf(gnuplot, "plot 'cartes1' with points ls 2, 'cartes2' with points ls 3");
 117
 118         fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 4 with linespoints",cartes[begin].x, cartes[begin].y, cartes[end].x, cartes[end].y);
 119         fprintf(gnuplot, ", \"< echo \'%f %f\'\" ls 1 pointsize 3 with points",cartes[maxdist].x, cartes[maxdist].y);
 120
 121         for (int i = 0; i < (int) lines.size(); i++) {
 122                 fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 1 with linespoints",lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
 123         }
 124
 125         fprintf(gnuplot, "\n");
 126         fflush(gnuplot);
 127         getchar();
 128 }
 129
 130 // line recursive fitting
 131 void line_fitting(int begin, int end, vector<Point> &cartes, vector<Line> &lines) {
 132         int line_break_point;
 133         //cout << "begin: " << begin << " end: " << end << endl << flush;
 134
 135         if ((end - begin) < LINE_MIN_POINTS) return;
 136
 137         float r;
 138         if (perpendicular_regression(r, line_break_point, begin, end, cartes)) return; // r = 0
 139
 140         if (r < LINE_ERROR_THRESHOLD) {
 141
 142                 Line line;
 143
 144                 line.a = cartes[begin];
 145                 line.b = cartes[end];
 146
 147                 lines.push_back(line);
 148
 149                 //cout << endl << "begin X: " << cartes[begin].x << " Y: " << cartes[begin].y << endl << flush;
 150                 //cout << "end X: " << cartes[end].x << " Y: " << cartes[end].y << endl << endl << flush;
 151         } else {
 152                 // Ax+By+C=0
 153                 // normal vector: n[n_x, -n_y]
 154 #if 1
 155                 float n_x = cartes[begin].y - cartes[end].y;
 156                 float n_y = cartes[begin].x - cartes[end].x;
 157
 158                 float A = n_x;
 159                 float B = -n_y;
 160                 float C = n_y*cartes[end].y - n_x*cartes[end].x;
 161
 162                 int line_break_point = 0;
 163                 float dist, dist_max = 0;
 164
 165                 for (int i = begin; i < end; i++) {
 166                         // distance point from the line
 167                         dist = fabs( (cartes[i].x*A + cartes[i].y*B + C) / sqrt(A*A + B*B));
 168
 169                         if (dist > dist_max) {
 170                                 dist_max = dist;
 171                                 line_break_point = i;
 172                         }
 173                 }
 174
 175                 //plot_line(begin, end, line_break_point, cartes, lines);
 176
 177                 if (dist_max > LINE_ERROR_THRESHOLD) {
 178                         line_fitting(begin, line_break_point, cartes, lines);
 179                         line_fitting(line_break_point, end, cartes, lines);
 180                 }
 181 #else
 182                 if (line_break_point == begin)
 183                         line_break_point++;
 184                 else if (line_break_point == end)
 185                         line_break_point--;
 186
 187                 line_fitting(begin, line_break_point, cartes, lines);
 188                 line_fitting(line_break_point, end, cartes, lines);
 189 #endif
 190         } // end if (r <= LINE_ERROR_THRESHOLD)
 191
 192         return;
 193 }
 194
 195 // polar to cartesian coordinates
 196 void polar_to_cartes(const vector<int> &input_data, vector<Point> &cartes) {
 197         Point point;
 198
 199         float fi;
 200         int r;
 201
 202         for (int i = 0; i < (int) input_data.size()-1; i++) {
 203                 r = (input_data[i] <= 19) ? 0 : input_data[i];
 204
 205                 if (r > 0) {
 206                         fi = HOKUYO_INDEX_TO_RAD(i);
 207                         point.x = r * cos(fi);
 208                         point.y = r * sin(fi);
 209                         cartes.push_back(point);
 210                 }
 211         }
 212 }
 213
 214 // save vector to file
 215 void file_save(const vector<Point> &data, string name) {
 216         char * file_name;
 217         file_name = new char[name.length() + 1];
 218         strcpy(file_name, name.c_str());
 219
 220         FILE *file = fopen(file_name,"w");
 221
 222         for (int i = 0; i < (int) data.size(); i++)
 223                 fprintf(file, "%f, %f\n", data[i].x, data[i].y);
 224
 225         fclose(file);
 226         delete file_name;
 227 }
 228
 229 float point_distance(Point a, Point b) {
 230         return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
 231 }
 232
 233 void shape_detect(const vector<int> &input_data, vector<Line> &lines) {
 234         // polar coordinates to cartesian coordinates
 235         vector<Point> cartes;
 236         polar_to_cartes(input_data, cartes);
 237
 238         int cartes_size = cartes.size();
 239
 240         int end, start = 0;
 241         while (start < cartes_size) {
 242                 end = start + 1;
 243
 244                 while (point_distance(cartes[end-1], cartes[end]) < MAX_DISTANCE_POINT && end < cartes_size)
 245                         end++;
 246
 247                 end--;
 248
 249                 line_fitting(start, end, cartes, lines);
 250                 start = end + 1;
 251         }
 252         fprintf(gnuplot, "set grid\n");
 253         fprintf(gnuplot, "set nokey\n");
 254         fprintf(gnuplot, "set style line 1 lt 2 lc rgb \"red\" lw 3\n");
 255
 256         fprintf(gnuplot, "plot [-1000:+3000] [-3000:+3000]");
 257         fprintf(gnuplot, "'-' with points ls 2"); // points
 258         fprintf(gnuplot, ", '-' ls 1 with linespoints"); // lines
 259         fprintf(gnuplot, "\n");
 260
 261         // points data
 262         for (int i = 0; i < (int) cartes.size(); i++) {
 263                 fprintf(gnuplot, "%g %g\n",cartes[i].x, cartes[i].y);
 264         }
 265         fprintf(gnuplot, "e\n");
 266
 267         // lines data
 268         for (int i = 0; i < (int) lines.size(); i++) {
 269                 fprintf(gnuplot, "%f %f\n%f %f\n\n",
 270                         lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
 271         }
 272         fprintf(gnuplot, "e\n");
 273         fflush(gnuplot);
 274 }
 275
 276 struct robottype_orte_data orte;
 277
 278 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 279                         void *recvCallBackParam)
 280 {
 281         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 282         static int count = 0;
 283
 284         switch (info->status) {
 285                 case NEW_DATA: {
 286                         printf("Scan\n");
 287                         if (++count >= 1) {
 288                                 printf("Detect\n");
 289                                 count = 0;
 290
 291                                 vector<int> input(HOKUYO_ARRAY_SIZE);
 292
 293                                 for(unsigned i = 0; i < HOKUYO_ARRAY_SIZE; i++)
 294                                         input[i] = (int) instance->data[HOKUYO_ARRAY_SIZE-1-i];
 295
 296                                 vector<Line> output;
 297                                 shape_detect(input, output);
 298                         }
 299                         break;
 300                 }
 301                 case DEADLINE:
 302                         printf("Deadline\n");
 303                         break;
 304         }
 305 }
 306
 307 int robot_init_orte()
 308 {
 309         int rv = 0;
 310
 311         rv = robottype_roboorte_init(&orte);
 312         if (rv) return rv;
 313
 314         robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
 315         return rv;
 316 }
 317
 318 #if 1
 319 int main(int argc, char** argv) {
 320
 321         if (argc < 2) {
 322                 cout << "Error: Invalid number of input parameters." << endl;
 323                 return 1;
 324         }
 325
 326         vector<int> input_data;
 327
 328         ifstream infile(argv[1], ios_base::in);
 329
 330         // line input file
 331         string line;
 332
 333         // number from input file
 334         int number;
 335
 336         int tmp = 1;
 337
 338         while (getline(infile, line, ',')) {
 339                 if (tmp) {
 340                         tmp = 0;
 341                         continue;
 342                 }
 343                 if (line != "\n") {
 344                         stringstream strStream(line);
 345                         strStream >> number;
 346                         input_data.push_back(number);
 347                         tmp = 1;
 348                 }
 349         }
 350
 351         vector<Line> output_data;
 352
 353         // detect line
 354         shape_detect(input_data, output_data);
 355
 356         return 0;
 357 }
 358 #else
 359 int main()
 360 {
 361         gnuplot = popen("gnuplot", "w");
 362         robot_init_orte();
 363         getchar();
 364         pclose(gnuplot);
 365         return 0;
 366 }
 367 #endif