src/hokuyo/shape-detect/shape_detect.h

   1 #ifndef SHAPE_DETECT
   2 #define SHAPE_DETECT
   3
   4 #include <string>
   5 #include <cstring>
   6 #include <stdlib.h>
   7 #include <fstream>
   8 #include <iostream>
   9 #include <sstream>
  10 #include <math.h>
  11 #include <vector>
  12 #include <hokuyo.h>
  13 #include <robot.h>
  14 #include <robomath.h>
  15 #include <robottype.h>
  16 #include <roboorte_robottype.h>
  17
  18 // gnuplot graph
  19 //#define GNUPLOT 0
  20
  21 // debug mode with connect Hokuyo
  22 #define OFFLINE 1
  23
  24 #define LINE_MIN_POINTS 7
  25 #define LINE_ERROR_THRESHOLD 20
  26 #define MAX_DISTANCE_POINT 300
  27
  28 class Shape_detect
  29 {
  30         public:
  31                 Shape_detect (void);
  32
  33                 typedef struct {float a,b,c;} General_form;     // Line equation - General form
  34                 typedef struct {float x,y;} Point;
  35                 typedef struct {Point a,b;} Line;
  36
  37                 void shape_detect(const std::vector<int> &input_data, std::vector<Line> &lines);
  38
  39         private:
  40 #ifdef GNUPLOT
  41                 FILE *gnuplot;
  42
  43                 void plot_line(int begin, int end, int maxdist, std::vector<Point> &cartes, std::vector<Line> &lines);
  44                 void plot_shape_detect(std::vector<Line> &lines, std::vector<Point> &cartes);
  45                 void gnuplot_init();
  46 #endif
  47
  48                 inline Point intersection_line(const Point point, const General_form gen);
  49
  50                 int perpendicular_regression(float &r, const int begin, const int end, std::vector<Point> &cartes, General_form &gen);
  51
  52                 void line_fitting(int begin, int end, std::vector<Point> &cartes, std::vector<Line> &lines);
  53
  54                 void polar_to_cartes(const std::vector<int> &input_data, std::vector<Point> &cartes);
  55
  56                 inline float point_distance(Point a, Point b);
  57 };
  58
  59 #endif // SHAPE_DETECT