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 Debugging
  45
  46 The debug mode permits detection of segment lines with connected laser
  47 scanner or from scanned data, which are saved in a file. There is a
  48 program @a shape_detect_offline, which takes the measured data
  49 (generated for example by @a hokuyo-dump command) and writes out the
  50 detected lines. It can also plot the results using gnuplot (-g
  51 option).
  52
  53 \section shapedet_example Example
  54
  55 In the file main.cc are introduced examples of using class Shape_detect.
  56 The first step is creation of classinstance by constructor calling
  57 (default setting for Hokuyo or with applicable parameters). After that it is possible
  58 preparation of output vector for saving founded segment line and calling method
  59 shape_detect with applicable parameters.
  60
  61 The first example is work illustration with connected laser scanner Hokuyo.
  62
  63 \include hokuyo/shape-detect/online.cc
  64
  65 \section shapedet_ref References
  66
  67 Fast line, arc/circle and leg detection from laser scan data in a Player driver,
  68 http://w3.ualg.pt/~dcastro/a1738.pdf
  69
  70 Mathpages, Perpendicular regression of a line,
  71 http://mathpages.com/home/kmath110.htm
  72
  73  */
  74
  75 #ifndef SHAPE_DETECT
  76 #define SHAPE_DETECT
  77
  78 #include <string>
  79 #include <cstring>
  80 #include <stdlib.h>
  81 #include <fstream>
  82 #include <iostream>
  83 #include <sstream>
  84 #include <math.h>
  85 #include <vector>
  86 #include <hokuyo.h>
  87 #include <robot.h>
  88 #include <robomath.h>
  89 #include <robottype.h>
  90 #include <roboorte_robottype.h>
  91
  92 /**
  93  * There are detected line segments in input array of measured data (laser_scan)
  94  * by using perpendicular line regression.
  95  * The output is formed by vector of type Line (so detected segment line - coordinates endpoints).
  96  * @ingroup shapedet
  97  */
  98 class Shape_detect
  99 {
 100         public:
 101                 /**
 102                  * The constructor with default setting of detection properties (pro Hokuyo).
 103                  * Line_min_points = 7
 104                  * Line_error_threshold = 20
 105                  * Max_distance_point = 300
 106                  * @ingroup shapedet
 107                  */
 108                 Shape_detect (void);
 109
 110                 /**
 111                  * The constructor for other setting than default setting of detection properties.
 112                  * @param line_min_points the minimal number of points which can create segment line.
 113                  * @param line_error_threshold the maximal pointerror from segment line of regression.
 114                  * @param max_distance_point the maximal Euclidean distance of point.
 115                  * @ingroup shapedet
 116                  */
 117                 Shape_detect (int line_min_points, int line_error_threshold, int max_distance_point);
 118
 119                 /**
 120                  * General equation of line -> Ax + By + C = 0.
 121                  * Is used for calculation of lines intersection.
 122                  * @ingroup shapedet
 123                  */
 124                 typedef struct {float a,b,c;} General_form;
 125
 126                 /**
 127                  * Point expressed in cartesian coordinates.
 128                  * @ingroup shapedet
 129                  */
 130                 typedef struct {
 131                         float x; /**< x coordinates point. */
 132                         float y; /**< y coordinates point. */
 133                 } Point;
 134
 135                 /**
 136                  * Line defined by two points which are expressed in cartesian coordinates.
 137                  * @ingroup shapedet
 138                  */
 139                 typedef struct {
 140                         Point a; /**< start point from a line. */
 141                         Point b; /**< end point from a line. */
 142                 } Line;
 143
 144                 /**
 145                  * Arc defined by TODO.
 146                  * @ingroup shapedet
 147                  */
 148                 typedef struct {
 149                         Point center;
 150                         Point begin;
 151                         Point end;
 152                         float radius;
 153                 } Arc;
 154
 155                 /**
 156                  * TODO
 157                  * @param [in] laser_scan contains laser scanned data.
 158                  * @ingroup shapedet
 159                  */
 160                 void prepare(const unsigned short laser_scan[]);
 161
 162                 /** Returns laser_scan data set by prepare converted to cartesian coordinates */
 163                 std::vector<Point> &getCartes();
 164
 165
 166                 /**
 167                  * There are detected line segments in input array of measured
 168                  * data by using perpendicular line regression.
 169                  * @param [out] &lines vector which contains detected lines.
 170                  * @ingroup shapedet
 171                  */
 172                 void line_detect(std::vector<Line> &lines);
 173
 174                 /**
 175                  * There are detected line segments in input array of measured
 176                  * data.
 177                  * @param [out] &arcs vector which contains detected arcs.
 178                  * @ingroup shapedet
 179                  */
 180                 void arc_detect(std::vector<Arc> &arcs);
 181
 182         private:
 183                 /**
 184                  * The minimal number of points which can create segment line.
 185                  * @ingroup shapedet
 186                  */
 187                 int Line_min_points;
 188
 189                 /**
 190                  * The maximal pointerror from segment line of regression.
 191                  * @ingroup shapedet
 192                  */
 193                 int Line_error_threshold;
 194
 195                 /**
 196                  * The maximal Euclidean distance of point.
 197                  * @ingroup shapedet
 198                  */
 199                 int Max_distance_point;
 200
 201                 std::vector<Point> cartes;//(HOKUYO_ARRAY_SIZE);
 202
 203                 float Arc_max_aperture;
 204                 float Arc_min_aperture;
 205                 float Arc_std_max;
 206
 207                 /**
 208                  * Calculation of lines intersection.
 209                  * @param point which pertains to line.
 210                  * @param gen is general equation of line.
 211                  * @return point of intersection.
 212                  * @ingroup shapedet
 213                  */
 214                 inline Point intersection_line(const Point point, const General_form gen);
 215
 216                 /**
 217                  * Calculating perpendicular regression of a line in input range index points.
 218                  * @param [out] &r is minimal distance between point and found line.
 219                  * @param [in] begin is start point.
 220                  * @param [in] end is last point.
 221                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 222                  * @param [out] &gen is general equation of found line.
 223                  * @return 0 for right course else 1.
 224                  * @ingroup shapedet
 225                  */
 226                 int perpendicular_regression(float &r, const int begin, const int end, General_form &gen);
 227
 228                 /**
 229                  * In case the input range points does not line. Is range divided and to single parts is again applied line_fitting function.
 230                  * @param [in] begin is start point.
 231                  * @param [in] end is last point.
 232                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 233                  * @param [out] &lines is vector with detecting lines.
 234                  * @ingroup shapedet
 235                  */
 236                 void line_fitting(int begin, int end, std::vector<Line> &lines);
 237
 238                 /**
 239                  * Convert vector expressed in polar coordinates to vector expressed in cartesian coordinates.
 240                  * @param laser_scan laser scanned data expressed in polar coordinates.
 241                  * @param &cartes is vector whose points are expressed in cartesian coordinates.
 242                  * @ingroup shapedet
 243                  */
 244                 void polar_to_cartes(const unsigned short laser_scan[]);
 245
 246                 /**
 247                  * Calculation of distance between points which are expressed in cartesian coordinates.
 248                  * @param a is first point.
 249                  * @param b is second point.
 250                  * @return distance between points.
 251                  * @ingroup shapedet
 252                  */
 253                 inline float point_distance(Point a, Point b);
 254
 255                 inline Point rotate(Point input_point, float rad);
 256
 257                 bool fit_arc(int begin, int end, std::vector<Arc> &arcs);
 258 };
 259
 260 #endif // SHAPE_DETECT