src/hokuyo/shape-detect/shape_detect.h

   1 /**
   2  * @file shape_detect.h
   3  * @author Martin Synek
   4  * @author Michal Sojka
   5  * @date 11/02/25
   6  *
   7  * @brief Shape detection from laser scan data
   8  */
   9
  10 /* Copyright: TODO
  11  *
  12  */
  13
  14 /**
  15 \defgroup shapedet Shape detection
  16
  17 Library Shape detect is used for detection line segments
  18 in measured data by laser scan (defaut constructor
  19 is changed for Hokuyo).
  20
  21 Content:
  22 - \ref shapedet_general
  23 - \ref shapedet_debug_mode
  24 - \ref shapedet_example
  25 - \ref shapedet_ref
  26
  27 \section shapedet_general Introduction
  28
  29 The input is created by array laser_scan (type unsigned short) which contains
  30 data expressed in polar coordinates. The first step is data conversion from
  31 polar coordinates to vector whose points are expressed in cartesian coordinates.
  32 The section includes filtering of error scanned data which are lower level then 20.
  33
  34 In the next is divided pointvector in dependence on parametr max_distance_point.
  35 Some line segment is searched by recursion in the individual parts of pointvector
  36 by perpendicular line regression.
  37 The line segment is detected or set is divided to achievement of parametersize
  38 line_min_points.
  39
  40 The founded line is written into output vector lines. The method shape_detect
  41 is finished after research of all vectorparts.
  42
  43 \section shapedet_debug_mode Debug mode
  44
  45 The debug mode permits detection of segment lines with connected laser scanner
  46 or from scanned data, which are saved in a file. GNUPLOT can be used for graphical
  47 output in debug mode or output vector can be saved in a file for next processing
  48 in any application.
  49
  50 For setting above properties is used directives define GNUPLOT and OFFLINE.
  51
  52 For debugging can be used methods introduced in file main.cc.
  53
  54 \section shapedet_example Example
  55
  56 In the file main.cc are introduced examples of using class Shape_detect.
  57 The first step is creation of classinstance by constructor calling
  58 (default setting for Hokuyo or with applicable parameters). After that it is possible
  59 preparation of output vector for saving founded segment line and calling method
  60 shape_detect with applicable parameters.
  61
  62 The first example is work illustration with connected laser scanner Hokuyo.
  63
  64 \code
  65 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
  66                         void *recvCallBackParam)
  67 {
  68         Shape_detect sd;
  69
  70         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
  71         static int count = 0;
  72
  73         switch (info->status) {
  74                 case NEW_DATA: {
  75                         printf("Scan\n");
  76                         if (++count >= 2) {
  77                                 printf("Detect\n");
  78                                 count = 0;
  79
  80                                 std::vector<Shape_detect::Line> output;
  81                                 sd.shape_detect((unsigned short) instance->data, output);
  82                         }
  83                         break;
  84                 }
  85                 case DEADLINE:
  86                         printf("Deadline\n");
  87                         break;
  88         }
  89 }
  90 \endcode
  91
  92 The second example shows values reading from datafile.
  93
  94 \code
  95 Shape_detect sd;
  96
  97 unsigned short laser_scan[HOKUYO_ARRAY_SIZE];
  98
  99 std::ifstream infile(argv[1], std::ios_base::in);
 100
 101 // line input file
 102 std::string line;
 103
 104 // number from input file
 105 int number;
 106
 107 int tmp = 1;
 108 int idx = 0;
 109
 110 while (std::getline(infile, line, ',')) {
 111         if (tmp) {
 112                 tmp = 0;
 113                 idx++;
 114                 continue;
 115         }
 116         if (line != "\n") {
 117                 std::stringstream strStream(line);
 118                 strStream >> number;
 119                 laser_scan[idx] = number;
 120                 tmp = 1;
 121         }
 122         idx++;
 123 }
 124
 125 std::vector<Shape_detect::Line> output_data;
 126 sd.shape_detect(laser_scan, output_data);
 127 \endcode
 128
 129 \section shapedet_ref References
 130
 131 Fast line, arc/circle and leg detection from laser scan data in a Player driver,
 132 http://w3.ualg.pt/~dcastro/a1738.pdf
 133
 134 Mathpages, Perpendicular regression of a line,
 135 http://mathpages.com/home/kmath110.htm
 136
 137  */
 138
 139 #ifndef SHAPE_DETECT
 140 #define SHAPE_DETECT
 141
 142 #include <string>
 143 #include <cstring>
 144 #include <stdlib.h>
 145 #include <fstream>
 146 #include <iostream>
 147 #include <sstream>
 148 #include <math.h>
 149 #include <vector>
 150 #include <hokuyo.h>
 151 #include <robot.h>
 152 #include <robomath.h>
 153 #include <robottype.h>
 154 #include <roboorte_robottype.h>
 155
 156 /**
 157  * Debug mode with GNUPLOT graph.
 158  * True  -> Detected lines are painted by GNUPLOT.
 159  * False -> Lines isn't painted.
 160  * @ingroup shapedet
 161  */
 162 #define GNUPLOT
 163
 164 /**
 165  * Debug mode with connect Hokuyo.
 166  * True  -> Hokuyo not connect and input data vector is reading from file.
 167  * False -> Hokuyo is connect.
 168  * @ingroup shapedet
 169  */
 170 #define OFFLINE 1
 171
 172 /**
 173  * There are detected line segments in input array of measured data (laser_scan)
 174  * by using perpendicular line regression.
 175  * The output is formed by vector of type Line (so detected segment line - coordinates endpoints).
 176  * @ingroup shapedet
 177  */
 178 class Shape_detect
 179 {
 180         public:
 181                 /**
 182                  * The constructor with default setting of detection properties (pro Hokuyo).
 183                  * Line_min_points = 7
 184                  * Line_error_threshold = 20
 185                  * Max_distance_point = 300
 186                  * @ingroup shapedet
 187                  */
 188                 Shape_detect (void);
 189
 190                 /**
 191                  * The constructor for other setting than default setting of detection properties.
 192                  * @param line_min_points the minimal number of points which can create segment line.
 193                  * @param line_error_threshold the maximal pointerror from segment line of regression.
 194                  * @param max_distance_point the maximal Euclidean distance of point.
 195                  * @ingroup shapedet
 196                  */
 197                 Shape_detect (int line_min_points, int line_error_threshold, int max_distance_point);
 198
 199                 /**
 200                  * General equation of line -> Ax + By + C = 0.
 201                  * Is used for calculation of lines intersection.
 202                  * @ingroup shapedet
 203                  */
 204                 typedef struct {float a,b,c;} General_form;
 205
 206                 /**
 207                  * Point expressed in cartesian coordinates.
 208                  * @ingroup shapedet
 209                  */
 210                 typedef struct {
 211                         float x; /**< x coordinates point. */
 212                         float y; /**< y coordinates point. */
 213                 } Point;
 214
 215                 /**
 216                  * Line defined by two points which are expressed in cartesian coordinates.
 217                  * @ingroup shapedet
 218                  */
 219                 typedef struct {
 220                         Point a; /**< start point from a line. */
 221                         Point b; /**< end point from a line. */
 222                 } Line;
 223
 224                 typedef struct {
 225                         Point center;
 226                 } Arc;
 227
 228                 void prepare(const unsigned short laser_scan[]);
 229
 230
 231                 /**
 232                  * There are detected line segments in input array of measured
 233                  * data by using perpendicular line regression.
 234                  * @param [in] laser_scan contains laser scanned data.
 235                  * @param [out] &lines vector which contains detected lines.
 236                  * @ingroup shapedet
 237                  */
 238                 void line_detect(std::vector<Line> &lines);
 239                 void arc_detect(std::vector<Arc> &lines);
 240
 241         private:
 242                 /**
 243                  * The minimal number of points which can create segment line.
 244                  * @ingroup shapedet
 245                  */
 246                 int Line_min_points;
 247
 248                 /**
 249                  * The maximal pointerror from segment line of regression.
 250                  * @ingroup shapedet
 251                  */
 252                 int Line_error_threshold;
 253
 254                 /**
 255                  * The maximal Euclidean distance of point.
 256                  * @ingroup shapedet
 257                  */
 258                 int Max_distance_point;
 259
 260                 float Arc_max_aperture;
 261                 float Arc_min_aperture;
 262                 float Arc_std_max;
 263
 264 #ifdef GNUPLOT
 265                 FILE *gnuplot;
 266
 267                 /**
 268                  * Method painted lines during the calculation in breaking point of GNUPLOT.
 269                  * It must be defined global flag GNUPLOT.
 270                  * @param begin is index of start point line.
 271                  * @param end is index of last point line.
 272                  * @param maxdist is index point of maximal distance from line.
 273                  * @param &cartes is vector whose points are expressed in cartesian coordinates.
 274                  * @param &lines is vector which contains detected lines.
 275                  * @ingroup shapedet
 276                  */
 277                 void plot_line(int begin, int end, int maxdist, std::vector<Point> &cartes, std::vector<Line> &lines);
 278
 279                 /**
 280                  * Method paints detection lines in GNUPLOT.
 281                  * For debug mode. It must be defined global flag GNUPLOT.
 282                  * @param &lines is vector which contains detected lines.
 283                  * @param &cartes is vector whose points are expressed in cartesian coordinates.
 284                  * @ingroup shapedet
 285                  */
 286                 void plot_shape_detect(std::vector<Line> &lines, std::vector<Point> &cartes, std::vector<Point> &arcs);
 287
 288                 /**
 289                  * Method for initialization GNUPLOT - opens pipe and performs basic settings.
 290                  * For debug mode. It must be defined global flag GNUPLOT.
 291                  * @ingroup shapedet
 292                  */
 293                 void gnuplot_init();
 294 #endif
 295                 /**
 296                  * Calculation of lines intersection.
 297                  * @param point which pertains to line.
 298                  * @param gen is general equation of line.
 299                  * @return point of intersection.
 300                  * @ingroup shapedet
 301                  */
 302                 inline Point intersection_line(const Point point, const General_form gen);
 303
 304                 /**
 305                  * Calculating perpendicular regression of a line in input range index points.
 306                  * @param [out] &r is minimal distance between point and found line.
 307                  * @param [in] begin is start point.
 308                  * @param [in] end is last point.
 309                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 310                  * @param [out] &gen is general equation of found line.
 311                  * @return 0 for right course else 1.
 312                  * @ingroup shapedet
 313                  */
 314                 int perpendicular_regression(float &r, const int begin, const int end, std::vector<Point> &cartes, General_form &gen);
 315
 316                 /**
 317                  * In case the input range points does not line. Is range divided and to single parts is again applied line_fitting function.
 318                  * @param [in] begin is start point.
 319                  * @param [in] end is last point.
 320                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 321                  * @param [out] &lines is vector with detecting lines.
 322                  * @ingroup shapedet
 323                  */
 324                 void line_fitting(int begin, int end, std::vector<Point> &cartes, std::vector<Line> &lines);
 325
 326                 /**
 327                  * Convert vector expressed in polar coordinates to vector expressed in cartesian coordinates.
 328                  * @param laser_scan laser scanned data expressed in polar coordinates.
 329                  * @param &cartes is vector whose points are expressed in cartesian coordinates.
 330                  * @ingroup shapedet
 331                  */
 332                 void polar_to_cartes(const unsigned short laser_scan[], std::vector<Point> &cartes);
 333
 334                 /**
 335                  * Calculation of distance between points which are expressed in cartesian coordinates.
 336                  * @param a is first point.
 337                  * @param b is second point.
 338                  * @return distance between points.
 339                  * @ingroup shapedet
 340                  */
 341                 inline float point_distance(Point a, Point b);
 342
 343                 inline Point rotate(Point input_point, float rad);
 344
 345                 bool fit_arc(int begin, int end, std::vector<Point> &cartes, std::vector<Point> &arcs);
 346 };
 347
 348 #endif // SHAPE_DETECT