src/hokuyo/shape-detect/offline.cc

   1 /**
   2  * @file
   3  * @author Martin Synek
   4  * @author Michal Sojka
   5  * @date 11/02/25
   6  *
   7  * @brief Debug file for testing detection shape.
   8  *
   9  * More information about using introduced methods
  10  * can be found in dokumentation of class Shape_detect (shape_detect.h)
  11  *
  12  * @ingroup shapedet
  13  */
  14
  15 /* Copyright: TODO
  16  *
  17  */
  18
  19
  20 #include <shape_detect.h>
  21 #include <iostream>
  22 #include <stdio.h>
  23 using namespace std;
  24
  25 ostream& operator << (ostream& os, Shape_detect::Point& point)
  26 {
  27         os << "(" << point.x << ", " << point.y << ")";
  28         return os;
  29 }
  30
  31
  32 ostream& operator << (ostream& os, Shape_detect::Line& line)
  33 {
  34         os << line.a << "--" << line.b;
  35         return os;
  36 }
  37
  38 ostream& operator << (ostream& os, Shape_detect::Arc& arc)
  39 {
  40         os << arc.center << " r=" << arc.radius;
  41         return os;
  42 }
  43
  44
  45
  46 void gnuplot(std::vector<Shape_detect::Point> &cartes,
  47              std::vector<Shape_detect::Line> &lines,
  48              std::vector<Shape_detect::Arc> &arcs)
  49 {
  50         FILE *gnuplot;
  51         gnuplot = popen("gnuplot", "w");
  52         fprintf(gnuplot, "set grid\n");
  53         fprintf(gnuplot, "set nokey\n");
  54         fprintf(gnuplot, "set size ratio -1\n"); //1.3333
  55         fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n");
  56         fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n");
  57         fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n");
  58         fprintf(gnuplot, "set style fill transparent solid 0.2 noborder\n");
  59         fprintf(gnuplot, "set palette model RGB functions .8-gray*2, .8-gray*2, .8-gray*2\n");
  60
  61         fprintf(gnuplot, "plot");
  62         for (unsigned i=0; i < arcs.size(); i++) {
  63                 if (arcs[i].debug)
  64                         fprintf(gnuplot, "'-' matrix using ($1*%g+%g):($2*%g+%g):3 with image, ",
  65                                 arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.x,
  66                                 arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.y);
  67         }
  68         if (cartes.size() > 0)
  69                 fprintf(gnuplot, "'-' with points ls 1");
  70         if (lines.size() > 0)
  71                 fprintf(gnuplot, ", '-' with linespoints ls 2");
  72         if (arcs.size() > 0)
  73                 fprintf(gnuplot, ", '-' with circles ls 2");
  74         fprintf(gnuplot, "\n");
  75
  76         // Draw accumulators
  77         for (int i = 0; i < (int) arcs.size(); i++) {
  78                 Shape_detect::Arc &a = arcs[i];
  79                 if (!a.debug)
  80                         continue;
  81                 for (int y=0; y < a.debug->acc_size; y++) {
  82                         for (int x=0; x < a.debug->acc_size; x++) {
  83                                 fprintf(gnuplot, "%d ", a.debug->bitmap[y*a.debug->acc_size+x]);
  84                         }
  85                         fprintf(gnuplot, "\n");
  86                 }
  87                 fprintf(gnuplot, "e\ne\n");
  88         }
  89
  90         if (cartes.size() > 0) {
  91                 for (int i = 0; i < (int) cartes.size(); i++) {
  92                         fprintf(gnuplot, "%g %g\n", cartes[i].x, cartes[i].y);
  93                 }
  94                 fprintf(gnuplot, "e\n");
  95         }
  96
  97         if (lines.size() > 0) {
  98                 for (int i = 0; i < (int) lines.size(); i++) {
  99                         fprintf(gnuplot, "%f %f\n%f %f\n\n",
 100                                 lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
 101                 }
 102                 fprintf(gnuplot, "e\n");
 103         }
 104         if (arcs.size() > 0) {
 105                 // Draw circles
 106                 for (int i = 0; i < (int) arcs.size(); i++) {
 107                         fprintf(gnuplot, "%f %f %f\n",
 108                                 arcs[i].center.x, arcs[i].center.y, arcs[i].radius);
 109                 }
 110                 fprintf(gnuplot, "e\n");
 111         }
 112
 113         fflush(gnuplot);
 114         getchar();
 115
 116         pclose(gnuplot);
 117 }
 118
 119 void get_laser_scan(unsigned short laser_scan[], const char *fname)
 120 {
 121
 122         std::ifstream infile(fname, std::ios_base::in);
 123
 124         // line input file
 125         std::string line;
 126
 127         // number from input file
 128         unsigned short number;
 129
 130         int idx = 0;
 131
 132         while (std::getline(infile, line, '\n')) {
 133                 std::stringstream strStream(line);
 134                 strStream >> number;
 135                 // Ignore out of range data (generated by simulator in robomon)
 136                 if (number >= 4000)
 137                         number = 0;
 138                 laser_scan[idx] = number;
 139                 idx++;
 140         }
 141 }
 142
 143 /**
 144  * There are detected line segments in input file (laser scan data)
 145  * @param argv name input file with measured data
 146  * @ingroup shapedet
 147  */
 148 int main(int argc, char** argv)
 149 {
 150         if (argc < 2 || strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0) {
 151                 std::cout << "Help:" << std::endl;
 152                 std::cout << "Example: ./shape_detect_offline -g -f file_name -n seq_length" << std::endl;
 153                 std::cout << "-g: Gnuplot output." << std::endl;
 154                 std::cout << "-f file_name example: seq-scan-<NUMBER>." << std::endl;
 155                 std::cout << "-n seq_length: Maximal length sequence file <NUMBER>." << std::endl;
 156                 std::cout << std::endl;
 157
 158                 return 0;
 159         }
 160
 161         Shape_detect sd;
 162
 163         bool plot = false;
 164         char fname[50];
 165         int number_file = 0;
 166         unsigned short laser_scan[HOKUYO_ARRAY_SIZE];
 167
 168         for (int i = 1; i < argc; i++) {
 169                 if (strcmp(argv[i], "-g") == 0)
 170                         plot = true;
 171                 else if (strcmp(argv[i], "-f") == 0) {
 172                         i++;
 173                         strcpy(fname, argv[i]);
 174                 } else if (strcmp(argv[i], "-n") == 0) {
 175                         i++;
 176                         number_file = atoi(argv[i]);
 177                 } else {
 178                         std::cout << "Invalid input parameter: " << argv[i] << "." << std::endl;
 179                         return 1;
 180                 }
 181         }
 182
 183         std::vector<Shape_detect::Line> lines;
 184         std::vector<Shape_detect::Arc> arcs;
 185         std::vector<Shape_detect::Arc> result;
 186
 187         if (number_file == 0) {
 188                 std::cout << "Open file: " << fname << std::endl;
 189                 get_laser_scan(laser_scan, fname);
 190
 191                 sd.prepare(laser_scan);
 192                 sd.line_detect(lines);
 193                 sd.arc_detect(result);
 194         } else {
 195                 for (int i = 1; i <= number_file; i++) {
 196                         char name[50];
 197                         if (i < 10)
 198                                 sprintf(name, "%s0%d", fname, i);
 199                         else
 200                                 sprintf(name, "%s%d", fname, i);
 201
 202                         std::cout << "Open file: " << name << std::endl;
 203
 204                         get_laser_scan(laser_scan, name);
 205
 206                         sd.prepare(laser_scan);
 207
 208                         if (i == 1) {
 209                                 sd.line_detect(lines);
 210                                 sd.arc_detect(result);
 211                                 continue;
 212                         }
 213
 214                         sd.arc_detect(arcs);
 215                         result = sd.arcs_compare(result, arcs, 8);
 216
 217                         if (result.size() == 0)
 218                                 break;
 219                 }
 220         }
 221
 222         std::cout << std::endl << "Result:" << std::endl;
 223
 224         for (unsigned i = 0; i < lines.size(); i++)
 225                 cout << "Line: " << lines[i] << endl;
 226
 227         for (unsigned i = 0; i < result.size(); i++)
 228                 cout << "Arc: " << result[i] << endl;
 229
 230         if (plot)
 231                 gnuplot(sd.getCartes(), lines, result);
 232
 233         return 0;
 234 }
 235