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                 struct arc_debug {
 145                         Point acc_origin;
 146                         float acc_scale;
 147                         int acc_size;
 148                         int *bitmap;
 149                 };
 150
 151                 /**
 152                  * Arc defined by TODO.
 153                  * @ingroup shapedet
 154                  */
 155                 typedef struct {
 156                         Point center;
 157                         Point begin;
 158                         Point end;
 159                         float radius;
 160                         struct arc_debug *debug;
 161                 } Arc;
 162
 163                 /**
 164                  * TODO
 165                  * @param [in] laser_scan contains laser scanned data.
 166                  * @ingroup shapedet
 167                  */
 168                 void prepare(const unsigned short laser_scan[]);
 169
 170                 /** Returns laser_scan data set by prepare converted to cartesian coordinates */
 171                 std::vector<Point> &getCartes();
 172
 173
 174                 /**
 175                  * There are detected line segments in input array of measured
 176                  * data by using perpendicular line regression.
 177                  * @param [out] &lines vector which contains detected lines.
 178                  * @ingroup shapedet
 179                  */
 180                 void line_detect(std::vector<Line> &lines);
 181
 182                 /**
 183                  * There are detected line segments in input array of measured
 184                  * data.
 185                  * @param [out] &arcs vector which contains detected arcs.
 186                  * @ingroup shapedet
 187                  */
 188                 void arc_detect(std::vector<Arc> &arcs);
 189
 190         private:
 191                 /**
 192                  * The minimal number of points which can create segment line.
 193                  * @ingroup shapedet
 194                  */
 195                 int Line_min_points;
 196
 197                 /**
 198                  * The maximal pointerror from segment line of regression.
 199                  * @ingroup shapedet
 200                  */
 201                 int Line_error_threshold;
 202
 203                 /**
 204                  * The maximal Euclidean distance of point.
 205                  * @ingroup shapedet
 206                  */
 207                 int Max_distance_point;
 208
 209                 std::vector<Point> cartes;//(HOKUYO_ARRAY_SIZE);
 210
 211                 float Arc_max_aperture;
 212                 float Arc_min_aperture;
 213                 float Arc_std_max;
 214
 215                 /**
 216                  * Calculation of lines intersection.
 217                  * @param point which pertains to line.
 218                  * @param gen is general equation of line.
 219                  * @return point of intersection.
 220                  * @ingroup shapedet
 221                  */
 222                 inline Point intersection_line(const Point point, const General_form gen);
 223
 224                 /**
 225                  * Calculating perpendicular regression of a line in input range index points.
 226                  * @param [out] &r is minimal distance between point and found line.
 227                  * @param [in] begin is start point.
 228                  * @param [in] end is last point.
 229                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 230                  * @param [out] &gen is general equation of found line.
 231                  * @return 0 for right course else 1.
 232                  * @ingroup shapedet
 233                  */
 234                 int perpendicular_regression(float &r, const int begin, const int end, General_form &gen);
 235
 236                 /**
 237                  * In case the input range points does not line. Is range divided and to single parts is again applied line_fitting function.
 238                  * @param [in] begin is start point.
 239                  * @param [in] end is last point.
 240                  * @param [in] &cartes is vector whose points are expressed in cartesian coordinates.
 241                  * @param [out] &lines is vector with detecting lines.
 242                  * @ingroup shapedet
 243                  */
 244                 void line_fitting(int begin, int end, std::vector<Line> &lines);
 245
 246                 /**
 247                  * Convert vector expressed in polar coordinates to vector expressed in cartesian coordinates.
 248                  * @param laser_scan laser scanned data expressed in polar coordinates.
 249                  * @param &cartes is vector whose points are expressed in cartesian coordinates.
 250                  * @ingroup shapedet
 251                  */
 252                 void polar_to_cartes(const unsigned short laser_scan[]);
 253
 254                 /**
 255                  * Calculation of distance between points which are expressed in cartesian coordinates.
 256                  * @param a is first point.
 257                  * @param b is second point.
 258                  * @return distance between points.
 259                  * @ingroup shapedet
 260                  */
 261                 inline float point_distance(Point a, Point b);
 262
 263                 inline Point rotate(Point input_point, float rad);
 264
 265                 bool fit_arc(int begin, int end, std::vector<Arc> &arcs);
 266
 267                 void hough_transform_arc(int begin, int end, std::vector<Arc> &arcs, float radius, float scale);
 268 };
 269
 270 #endif // SHAPE_DETECT