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.
  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 \include hokuyo/shape-detect/online.cc
  66
  67 \section shapedet_ref References
  68
  69 Fast line, arc/circle and leg detection from laser scan data in a Player driver,
  70 http://w3.ualg.pt/~dcastro/a1738.pdf
  71
  72 Mathpages, Perpendicular regression of a line,
  73 http://mathpages.com/home/kmath110.htm
  74
  75  */
  76
  77 #ifndef SHAPE_DETECT
  78 #define SHAPE_DETECT
  79
  80 #include <string>
  81 #include <cstring>
  82 #include <stdlib.h>
  83 #include <fstream>
  84 #include <iostream>
  85 #include <sstream>
  86 #include <math.h>
  87 #include <vector>
  88 #include <hokuyo.h>
  89 #include <robot.h>
  90 #include <robomath.h>
  91 #include <robottype.h>
  92 #include <roboorte_robottype.h>
  93
  94 /**
  95  * Debug mode with GNUPLOT graph.
  96  * True  -> Detected lines are painted by GNUPLOT.
  97  * False -> Lines isn't painted.
  98  * @ingroup shapedet
  99  */
 100 //#define GNUPLOT
 101
 102 /**
 103  * There are detected line segments in input array of measured data (laser_scan)
 104  * by using perpendicular line regression.
 105  * The output is formed by vector of type Line (so detected segment line - coordinates endpoints).
 106  * @ingroup shapedet
 107  */
 108 class Shape_detect
 109 {
 110         public:
 111                 /**
 112                  * The constructor with default setting of detection properties (pro Hokuyo).
 113                  * Line_min_points = 7
 114                  * Line_error_threshold = 20
 115                  * Max_distance_point = 300
 116                  * @ingroup shapedet
 117                  */
 118                 Shape_detect (void);
 119
 120                 /**
 121                  * The constructor for other setting than default setting of detection properties.
 122                  * @param line_min_points the minimal number of points which can create segment line.
 123                  * @param line_error_threshold the maximal pointerror from segment line of regression.
 124                  * @param max_distance_point the maximal Euclidean distance of point.
 125                  * @ingroup shapedet
 126                  */
 127                 Shape_detect (int line_min_points, int line_error_threshold, int max_distance_point);
 128
 129                 /**
 130                  * General equation of line -> Ax + By + C = 0.
 131                  * Is used for calculation of lines intersection.
 132                  * @ingroup shapedet
 133                  */
 134                 typedef struct {float a,b,c;} General_form;
 135
 136                 /**
 137                  * Point expressed in cartesian coordinates.
 138                  * @ingroup shapedet
 139                  */
 140                 typedef struct {
 141                         float x; /**< x coordinates point. */
 142                         float y; /**< y coordinates point. */
 143                 } Point;
 144
 145                 /**
 146                  * Line defined by two points which are expressed in cartesian coordinates.
 147                  * @ingroup shapedet
 148                  */
 149                 typedef struct {
 150                         Point a; /**< start point from a line. */
 151                         Point b; /**< end point from a line. */
 152                 } Line;
 153
 154                 /**
 155                  * Arc defined by TODO.
 156                  * @ingroup shapedet
 157                  */
 158                 typedef struct {
 159                         Point center;
 160                         Point begin;
 161                         Point end;
 162                         float radius;
 163                 } Arc;
 164
 165                 /**
 166                  * TODO
 167                  * @param [in] laser_scan contains laser scanned data.
 168                  * @ingroup shapedet
 169                  */
 170                 void prepare(const unsigned short laser_scan[]);
 171
 172                 /**
 173                  * There are detected line segments in input array of measured
 174                  * data by using perpendicular line regression.
 175                  * @param [out] &lines vector which contains detected lines.
 176                  * @ingroup shapedet
 177                  */
 178                 void line_detect(std::vector<Line> &lines);
 179
 180                 /**
 181                  * There are detected line segments in input array of measured
 182                  * data.
 183                  * @param [out] &arcs vector which contains detected arcs.
 184                  * @ingroup shapedet
 185                  */
 186                 void arc_detect(std::vector<Arc> &arcs);
 187
 188         private:
 189                 /**
 190                  * The minimal number of points which can create segment line.
 191                  * @ingroup shapedet
 192                  */
 193                 int Line_min_points;
 194
 195                 /**
 196                  * The maximal pointerror from segment line of regression.
 197                  * @ingroup shapedet
 198                  */
 199                 int Line_error_threshold;
 200
 201                 /**
 202                  * The maximal Euclidean distance of point.
 203                  * @ingroup shapedet
 204                  */
 205                 int Max_distance_point;
 206
 207                 std::vector<Point> cartes;//(HOKUYO_ARRAY_SIZE);
 208
 209                 float Arc_max_aperture;
 210                 float Arc_min_aperture;
 211                 float Arc_std_max;
 212
 213 #ifdef GNUPLOT
 214                 FILE *gnuplot;
 215
 216                 /**
 217                  * Method paints detection lines in GNUPLOT.
 218                  * For debug mode. It must be defined global flag GNUPLOT.
 219                  * @param &lines is vector which contains detected lines.
 220                  * @param &cartes is vector whose points are expressed in cartesian coordinates.
 221                  * @ingroup shapedet
 222                  */
 223                 void plot_line(std::vector<Line> &lines);
 224                 void plot_arc(std::vector<Arc> &arcs);
 225
 226                 /**
 227                  * Method for initialization GNUPLOT - opens pipe and performs basic settings.
 228                  * For debug mode. It must be defined global flag GNUPLOT.
 229                  * @ingroup shapedet
 230                  */
 231                 void plot_init();
 232                 void plot_close();
 233 #endif
 234                 /**
 235                  * Calculation of lines intersection.
 236                  * @param point which pertains to line.
 237                  * @param gen is general equation of line.
 238                  * @return point of intersection.
 239                  * @ingroup shapedet
 240                  */
 241                 inline Point intersection_line(const Point point, const General_form gen);
 242
 243                 /**
 244                  * Calculating perpendicular regression of a line in input range index points.
 245                  * @param [out] &r is minimal distance between point and found line.
 246                  * @param [in] begin is start point.
 247                  * @param [in] end is last point.
 248                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 249                  * @param [out] &gen is general equation of found line.
 250                  * @return 0 for right course else 1.
 251                  * @ingroup shapedet
 252                  */
 253                 int perpendicular_regression(float &r, const int begin, const int end, General_form &gen);
 254
 255                 /**
 256                  * In case the input range points does not line. Is range divided and to single parts is again applied line_fitting function.
 257                  * @param [in] begin is start point.
 258                  * @param [in] end is last point.
 259                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 260                  * @param [out] &lines is vector with detecting lines.
 261                  * @ingroup shapedet
 262                  */
 263                 void line_fitting(int begin, int end, std::vector<Line> &lines);
 264
 265                 /**
 266                  * Convert vector expressed in polar coordinates to vector expressed in cartesian coordinates.
 267                  * @param laser_scan laser scanned data expressed in polar coordinates.
 268                  * @param &cartes is vector whose points are expressed in cartesian coordinates.
 269                  * @ingroup shapedet
 270                  */
 271                 void polar_to_cartes(const unsigned short laser_scan[]);
 272
 273                 /**
 274                  * Calculation of distance between points which are expressed in cartesian coordinates.
 275                  * @param a is first point.
 276                  * @param b is second point.
 277                  * @return distance between points.
 278                  * @ingroup shapedet
 279                  */
 280                 inline float point_distance(Point a, Point b);
 281
 282                 inline Point rotate(Point input_point, float rad);
 283
 284                 bool fit_arc(int begin, int end, std::vector<Arc> &arcs);
 285 };
 286
 287 #endif // SHAPE_DETECT