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