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