src/hokuyo/shape-detect/shape_detect.cc

   1 /**
   2  * @file shape_detect.cc
   3  * @author Martin Synek
   4  * @author Michal Sojka
   5  * @date 11/02/25
   6  *
   7  * @ingroup shapedet
   8  */
   9
  10 /* Copyright: TODO
  11  *
  12  */
  13
  14 #include "shape_detect.h"
  15
  16 #ifdef GNUPLOT
  17 void Shape_detect::plot_line(std::vector<Shape_detect::Line> &lines)
  18 {
  19         fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n");
  20         fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n");
  21         fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n");
  22
  23 #ifdef OFFLINE
  24         fprintf(gnuplot, "plot");
  25 #else
  26         fprintf(gnuplot, "plot [-1000:+3000] [-3000:+3000]");
  27 #endif
  28         fprintf(gnuplot, "'-' with points ls 1, "); // points
  29         fprintf(gnuplot, "'-' with linespoints ls 2,"); // lines
  30         fprintf(gnuplot, "'-' with points ls 3"); //arc point
  31         fprintf(gnuplot, "\n");
  32
  33         // points data
  34         for (int i = 0; i < (int) cartes.size(); i++) {
  35                 fprintf(gnuplot, "%g %g\n", cartes[i].x, cartes[i].y);
  36         }
  37         fprintf(gnuplot, "e\n");
  38
  39         // lines data
  40         for (int i = 0; i < (int) lines.size(); i++) {
  41                 fprintf(gnuplot, "%f %f\n%f %f\n\n",
  42                         lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
  43         }
  44         fprintf(gnuplot, "e\n");
  45 }
  46
  47 void Shape_detect::plot_arc(std::vector<Shape_detect::Arc> &arcs)
  48 {
  49         // arcs data
  50         for (int i = 0; i < (int) arcs.size(); i++) {
  51                 fprintf(gnuplot, "%g %g\n", arcs[i].begin.x, arcs[i].begin.y);
  52                 fprintf(gnuplot, "%g %g\n", arcs[i].end.x, arcs[i].end.y);
  53         }
  54         fprintf(gnuplot, "e\n");
  55
  56         plot_close();
  57 }
  58
  59 void Shape_detect::plot_close()
  60 {
  61         fflush(gnuplot);
  62         getchar();
  63         pclose(gnuplot);
  64 }
  65
  66 void Shape_detect::plot_init()
  67 {
  68         gnuplot = popen("gnuplot", "w");
  69         fprintf(gnuplot, "set grid\n");
  70         fprintf(gnuplot, "set nokey\n");
  71 #ifdef OFFLINE
  72         fprintf(gnuplot, "set size ratio 1\n"); //1.3333
  73 #endif
  74 }
  75 #endif // GNUPLOT
  76
  77 Shape_detect::Shape_detect()
  78 {
  79         Shape_detect::Line_min_points = 7;
  80         Shape_detect::Line_error_threshold = 20;
  81         Shape_detect::Max_distance_point = 300;
  82
  83         Shape_detect::Arc_std_max = 0.2;//0.15
  84         Shape_detect::Arc_min_aperture = 1.57; //90 degrees
  85         Shape_detect::Arc_max_aperture = 2.365; //2.365 #135 degrees
  86 }
  87
  88 Shape_detect::Shape_detect(int line_min_points, int line_error_threshold, int max_distance_point)
  89 {
  90         Shape_detect::Line_min_points = line_min_points;
  91         Shape_detect::Line_error_threshold = line_error_threshold;
  92         Shape_detect::Max_distance_point = max_distance_point;
  93 }
  94
  95 inline Shape_detect::Point Shape_detect::intersection_line(const Shape_detect::Point point, const General_form gen)
  96 {
  97         Shape_detect::Point tmp;
  98
  99         tmp.x = (gen.b*gen.b*point.x - gen.a*gen.b*point.y - gen.a*gen.c) / (gen.a*gen.a + gen.b*gen.b);
 100         tmp.y = (gen.b*tmp.x - gen.b*point.x + gen.a*point.y) / gen.a;
 101
 102         return tmp;
 103 }
 104
 105 inline Shape_detect::Point Shape_detect::rotate(Shape_detect::Point input_point, float rad)
 106 {
 107         Shape_detect::Point tmp;
 108         tmp.x = input_point.x * cos(rad) - input_point.y * sin(rad);
 109         tmp.y = input_point.x * sin(rad) + input_point.y * cos(rad);
 110
 111         return tmp;
 112 }
 113
 114 inline float Shape_detect::point_distance(Shape_detect::Point a, Shape_detect::Point b)
 115 {
 116         return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
 117 }
 118
 119 bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &arcs)
 120 {
 121         //std::cout << "rozsah: " << begin << " - " << end << " -> ";
 122
 123         if (end-begin < 4) {
 124                 //std::cout << "mensi nez 4" << std::endl;
 125                 return false;
 126         }
 127
 128         Point rotated_point = cartes[(end + begin) / 2];
 129         Point right = cartes[begin];
 130         Point left = cartes[end];
 131         rotated_point.x = rotated_point.x - left.x;
 132         rotated_point.y = rotated_point.y - left.y;
 133         float angle_to_rotate = atan2((left.x - right.x) / (left.y - right.y),1);
 134         rotated_point = rotate(rotated_point, angle_to_rotate);
 135
 136         //std::cout << "rotacni bod: " << rotated_point.x << ", " << rotated_point.y << "; vzdalenost: " << Shape_detect::point_distance(left, right) << "; ";
 137
 138         if (-rotated_point.x < .1 * Shape_detect::point_distance(left, right) &&
 139                 -rotated_point.x > Shape_detect::point_distance(left, right)) {
 140                 //std::cout << "rotacni bod" << "; ";
 141                 return false;
 142         }
 143
 144         int segment_size = end - begin + 1;
 145         int slopes_size = segment_size - 3;
 146
 147         float ma, mb, slopes[slopes_size];
 148
 149         for (int i = 0; i < slopes_size; i++) {
 150                 Shape_detect::Point tmp = cartes[begin + i + 1];
 151                 ma = atan2(left.y - tmp.y, left.x - tmp.x);
 152                 mb = atan2(right.y - tmp.y, right.x - tmp.x);
 153
 154                 slopes[i] = ma - mb;
 155         }
 156
 157         float sum = 0;
 158         for (int i = 0; i < slopes_size; i++)
 159                 sum = sum + slopes[i];
 160
 161         float average = sum / slopes_size;
 162
 163         float totalvar = 0;
 164         int count = slopes_size;
 165
 166         while (count > 0) {
 167                 count--;
 168                 totalvar = totalvar + ((slopes[count] - average) * (slopes[count] - average));
 169         }
 170
 171         float standard_deviation = sqrt(totalvar / --slopes_size);
 172
 173         //std::cout << "std: " << standard_deviation << ", Average: " << average << ": ";
 174
 175         if (standard_deviation < Arc_std_max && Arc_max_aperture > abs(average) && abs(average) > Arc_min_aperture) {
 176                 Shape_detect::Point tmp;
 177                 tmp.x = right.x - left.x;
 178                 tmp.y = right.y - left.y;
 179
 180                 angle_to_rotate = atan2(tmp.y, tmp.x);
 181
 182                 tmp = Shape_detect::rotate(tmp, -angle_to_rotate);
 183
 184                 float middle = tmp.x / 2.0;
 185                 float height = middle * tan(average - M_PI / 2.0);
 186
 187                 Shape_detect::Point center;
 188                 center.x = middle;
 189                 center.y = height;
 190
 191                 center = Shape_detect::rotate(center, angle_to_rotate);
 192                 center.x = center.x + left.x;
 193                 center.y = center.y + left.y;
 194
 195                 Shape_detect::Arc arc;
 196                 arc.center = center;
 197                 arc.begin = cartes[begin];
 198                 arc.end = cartes[end];
 199
 200                 arc.radius = 0;
 201                 for (int i = 0; i < segment_size; i++) {
 202                         tmp.x = cartes[begin + i].x;
 203                         tmp.y = cartes[begin + i].y;
 204                         arc.radius = arc.radius + float(sqrt((tmp.x - center.x)*(tmp.x - center.x)+(tmp.y - center.y)*(tmp.y - center.y)));
 205                 }
 206
 207                 arc.radius = arc.radius / segment_size;
 208
 209                 arcs.push_back(arc);
 210
 211                 //std::cout << "center: " << center.x << "; " << center.y << " >> ";
 212
 213                 //std::cout << "Detekovan" << std::endl;
 214                 return 1;
 215         }
 216
 217         //fit_arc(begin + 1, end - 1, arcs);
 218
 219         //std::cout << "Nic" << std::endl;
 220
 221         return 0;
 222 }
 223
 224 int Shape_detect::perpendicular_regression(float &r, const int begin, const int end, General_form &gen)
 225 {
 226         int number_points = abs(end-begin) + 1;
 227
 228         if (number_points <= 0) return 1;
 229
 230         float sum_x = 0;
 231         float sum_y = 0;
 232
 233         for (int i = begin; i <= end; i++) {
 234                 sum_x = sum_x + cartes[i].x;
 235                 sum_y = sum_y + cartes[i].y;
 236         }
 237
 238         float med_x = sum_x / number_points;
 239         float med_y = sum_y / number_points;
 240
 241         Shape_detect::Point tmp;
 242
 243         float A = 0;
 244         float sum_xy = 0;
 245
 246         for (int i = begin; i <= end; i++) {
 247                 tmp.x = cartes[i].x - med_x;
 248                 tmp.y = cartes[i].y - med_y;
 249                 A = A + (tmp.x*tmp.x - tmp.y*tmp.y);
 250                 sum_xy = sum_xy + tmp.x * tmp.y;
 251         }
 252
 253         if (sum_xy == 0) sum_xy = 1e-8;
 254
 255         A = A / sum_xy;
 256
 257         // tan(q)^2 + A*tan(q) - 1 = 0 ( tan(q) sign as m ) -> quadratic equation
 258         float m1 = (-A + sqrt(A*A + 4)) / 2;
 259         float m2 = (-A - sqrt(A*A + 4)) / 2;
 260
 261         float b1 = med_y - m1*med_x;
 262         float b2 = med_y - m2*med_x;
 263
 264         // maximum error
 265         r = 0;
 266         unsigned ir = -1;
 267         float dist;
 268
 269         float r1 = 0;
 270         unsigned ir1 = -1;
 271         float dist1;
 272
 273         for (int i = begin; i < end; i++) {
 274                 // distance point from the line (A = m1, B = -1, C = b1)
 275                 dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) );
 276                 dist1 = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
 277
 278                 if (dist1 > r1) {
 279                         r1 = dist1;
 280                         ir1 = i;
 281                 }
 282
 283                 if (dist > r) {
 284                         r = dist;
 285                         ir = i;
 286                 }
 287         }
 288
 289         if (r < r1) {
 290                 gen.a = m1;
 291                 gen.c = b1;
 292         } else {
 293                 r = r1;
 294                 gen.a = m2;
 295                 gen.c = b2;
 296         }
 297
 298         gen.b = -1.0;
 299
 300         return 0;
 301 }
 302
 303         // line recursive fitting
 304 void Shape_detect::line_fitting(int begin, int end, std::vector<Shape_detect::Line> &lines)
 305 {
 306         if ((end - begin) < Shape_detect::Line_min_points) return;
 307
 308         float r;
 309         General_form gen;
 310
 311         if (perpendicular_regression(r, begin, end, gen)) return; // r = 0
 312
 313         if (r < Shape_detect::Line_error_threshold) {
 314                 Shape_detect::Line tmp;
 315
 316                 tmp.a = intersection_line(cartes[begin], gen);
 317                 tmp.b = intersection_line(cartes[end], gen);
 318
 319                 lines.push_back(tmp);
 320         } else {
 321                 // Ax+By+C=0
 322                 // normal vector: n[n_x, -n_y]
 323
 324                 float n_x = cartes[begin].y - cartes[end].y;
 325                 float n_y = cartes[begin].x - cartes[end].x;
 326
 327                 float A = n_x;
 328                 float B = -n_y;
 329                 float C = n_y*cartes[end].y - n_x*cartes[end].x;
 330
 331                 int line_break_point = 0;
 332                 float dist, dist_max = 0;
 333
 334                 for (int i = begin; i < end; i++) {
 335                         // distance point from the line
 336                         dist = fabs( (cartes[i].x*A + cartes[i].y*B + C) / sqrt(A*A + B*B));
 337
 338                         if (dist > dist_max) {
 339                                 dist_max = dist;
 340                                 line_break_point = i;
 341                         }
 342                 }
 343
 344                 if (dist_max > Shape_detect::Line_error_threshold) {
 345                         line_fitting(begin, line_break_point, lines);
 346                         line_fitting(line_break_point, end, lines);
 347                 }
 348
 349         } // end if (r <= Line_error_threshold)
 350
 351         return;
 352 }
 353
 354 // polar to cartesian coordinates
 355 void Shape_detect::polar_to_cartes(const unsigned short laser_scan[])
 356 {
 357         Shape_detect::Point point;
 358
 359         float fi;
 360         int r;
 361
 362         for (int i = 0; i < (int) HOKUYO_ARRAY_SIZE; i++) {
 363                 r = (laser_scan[i] <= 19) ? 0 : laser_scan[i];
 364
 365                 if (r > 0) {
 366                         fi = HOKUYO_INDEX_TO_RAD(i);
 367                         point.x = r * cos(fi);
 368                         point.y = r * sin(fi);
 369                         cartes.push_back(point);
 370                 }
 371         }
 372
 373         //std::cout << "velikost cartes: " << cartes.size() << std::endl;
 374
 375 }
 376
 377 void Shape_detect::prepare(const unsigned short laser_scan[])
 378 {
 379 #ifdef GNUPLOT
 380         plot_init();
 381 #endif
 382         polar_to_cartes(laser_scan);
 383 }
 384
 385 void Shape_detect::arc_detect(std::vector<Shape_detect::Arc> &arcs)
 386 {
 387         int cartes_size = cartes.size();
 388
 389         int end, start = 0;
 390         while (start < cartes_size) {
 391                 end = start + 1;
 392
 393                 while (point_distance(cartes[end-1], cartes[end]) < 50 && end < cartes_size)
 394                         end++;
 395
 396                 end --;
 397
 398                 fit_arc(start, end, arcs);
 399                 start = end + 1;
 400         }
 401 #ifdef GNUPLOT
 402         plot_arc(arcs);
 403 #endif
 404 }
 405
 406 void Shape_detect::line_detect(std::vector<Shape_detect::Line> &lines)
 407 {
 408         int cartes_size = cartes.size();
 409
 410         int end, start = 0;
 411         while (start < cartes_size) {
 412                 end = start + 1;
 413
 414                 while (point_distance(cartes[end-1], cartes[end]) < Shape_detect::Max_distance_point && end < cartes_size)
 415                         end++;
 416
 417                 end--;
 418
 419                 line_fitting(start, end, lines);
 420                 start = end + 1;
 421         }
 422 #ifdef GNUPLOT
 423         plot_line(lines);
 424 #endif
 425 }
 426