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