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 int Shape_detect::perpendicular_regression(float &r, const int begin, const int end, General_form &gen)
 165 {
 166         int number_points = abs(end-begin) + 1;
 167
 168         if (number_points <= 0) return 1;
 169
 170         float sum_x = 0;
 171         float sum_y = 0;
 172
 173         for (int i = begin; i <= end; i++) {
 174                 sum_x = sum_x + cartes[i].x;
 175                 sum_y = sum_y + cartes[i].y;
 176         }
 177
 178         float med_x = sum_x / number_points;
 179         float med_y = sum_y / number_points;
 180
 181         Shape_detect::Point tmp;
 182
 183         float A = 0;
 184         float sum_xy = 0;
 185
 186         for (int i = begin; i <= end; i++) {
 187                 tmp.x = cartes[i].x - med_x;
 188                 tmp.y = cartes[i].y - med_y;
 189                 A = A + (tmp.x*tmp.x - tmp.y*tmp.y);
 190                 sum_xy = sum_xy + tmp.x * tmp.y;
 191         }
 192
 193         if (sum_xy == 0) sum_xy = 1e-8;
 194
 195         A = A / sum_xy;
 196
 197         // tan(q)^2 + A*tan(q) - 1 = 0 ( tan(q) sign as m ) -> quadratic equation
 198         float m1 = (-A + sqrt(A*A + 4)) / 2;
 199         float m2 = (-A - sqrt(A*A + 4)) / 2;
 200
 201         float b1 = med_y - m1*med_x;
 202         float b2 = med_y - m2*med_x;
 203
 204         // maximum error
 205         r = 0;
 206         unsigned ir = -1;
 207         float dist;
 208
 209         float r1 = 0;
 210         unsigned ir1 = -1;
 211         float dist1;
 212
 213         for (int i = begin; i < end; i++) {
 214                 // distance point from the line (A = m1, B = -1, C = b1)
 215                 dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) );
 216                 dist1 = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
 217
 218                 if (dist1 > r1) {
 219                         r1 = dist1;
 220                         ir1 = i;
 221                 }
 222
 223                 if (dist > r) {
 224                         r = dist;
 225                         ir = i;
 226                 }
 227         }
 228
 229         if (r < r1) {
 230                 gen.a = m1;
 231                 gen.c = b1;
 232         } else {
 233                 r = r1;
 234                 gen.a = m2;
 235                 gen.c = b2;
 236         }
 237
 238         gen.b = -1.0;
 239
 240         return 0;
 241 }
 242
 243         // line recursive fitting
 244 void Shape_detect::line_fitting(int begin, int end, std::vector<Shape_detect::Line> &lines)
 245 {
 246         if ((end - begin) < Shape_detect::Line_min_points) return;
 247
 248         float r;
 249         General_form gen;
 250
 251         if (perpendicular_regression(r, begin, end, gen)) return; // r = 0
 252
 253         if (r < Shape_detect::Line_error_threshold) {
 254                 Shape_detect::Line tmp;
 255
 256                 tmp.a = intersection_line(cartes[begin], gen);
 257                 tmp.b = intersection_line(cartes[end], gen);
 258
 259                 lines.push_back(tmp);
 260         } else {
 261                 // Ax+By+C=0
 262                 // normal vector: n[n_x, -n_y]
 263
 264                 float n_x = cartes[begin].y - cartes[end].y;
 265                 float n_y = cartes[begin].x - cartes[end].x;
 266
 267                 float A = n_x;
 268                 float B = -n_y;
 269                 float C = n_y*cartes[end].y - n_x*cartes[end].x;
 270
 271                 int line_break_point = 0;
 272                 float dist, dist_max = 0;
 273
 274                 for (int i = begin; i < end; i++) {
 275                         // distance point from the line
 276                         dist = fabs( (cartes[i].x*A + cartes[i].y*B + C) / sqrt(A*A + B*B));
 277
 278                         if (dist > dist_max) {
 279                                 dist_max = dist;
 280                                 line_break_point = i;
 281                         }
 282                 }
 283
 284                 if (dist_max > Shape_detect::Line_error_threshold) {
 285                         line_fitting(begin, line_break_point, lines);
 286                         line_fitting(line_break_point, end, lines);
 287                 }
 288
 289         } // end if (r <= Line_error_threshold)
 290
 291         return;
 292 }
 293
 294 // polar to cartesian coordinates
 295 void Shape_detect::polar_to_cartes(const unsigned short laser_scan[])
 296 {
 297         Shape_detect::Point point;
 298
 299         float fi;
 300         int r;
 301
 302         for (int i = 0; i < (int) HOKUYO_ARRAY_SIZE; i++) {
 303                 r = (laser_scan[i] <= 19) ? 0 : laser_scan[i];
 304
 305                 if (r > 0) {
 306                         fi = HOKUYO_INDEX_TO_RAD(i);
 307                         point.x = r * cos(fi);
 308                         point.y = r * sin(fi);
 309                         cartes.push_back(point);
 310                 }
 311         }
 312
 313         //std::cout << "velikost cartes: " << cartes.size() << std::endl;
 314
 315 }
 316
 317 std::vector<Shape_detect::Point> &Shape_detect::getCartes()
 318 {
 319         return cartes;
 320 }
 321
 322 void Shape_detect::prepare(const unsigned short laser_scan[])
 323 {
 324         polar_to_cartes(laser_scan);
 325 }
 326
 327 void Shape_detect::arc_detect(std::vector<Shape_detect::Arc> &arcs)
 328 {
 329         int cartes_size = cartes.size();
 330
 331         int end, start = 0;
 332         while (start < cartes_size) {
 333                 end = start + 1;
 334
 335                 while (point_distance(cartes[end-1], cartes[end]) < 50 && end < cartes_size)
 336                         end++;
 337
 338                 end --;
 339
 340                 fit_arc(start, end, arcs);
 341                 start = end + 1;
 342         }
 343 }
 344
 345 void Shape_detect::line_detect(std::vector<Shape_detect::Line> &lines)
 346 {
 347         int cartes_size = cartes.size();
 348
 349         int end, start = 0;
 350         while (start < cartes_size) {
 351                 end = start + 1;
 352
 353                 while (point_distance(cartes[end-1], cartes[end]) < Shape_detect::Max_distance_point && end < cartes_size)
 354                         end++;
 355
 356                 end--;
 357
 358                 line_fitting(start, end, lines);
 359                 start = end + 1;
 360         }
 361 }
 362