From 9523741f367ec1f548786097b46cfb3c29865d55 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Thu, 17 Mar 2011 10:53:45 +0100 Subject: [PATCH] shapedet: Allow better testing To test shapedet behaviour, the following steps were done: - gnuplot support was removed from shape_det library - gnuplot support was added to shape_detect_offline test program and plotting is catvated by -g switch. - shape_detect_offline prints the detected objects to stdout With these changes, it is possible to test shape detector with individual data sets and tune the results based on this. --- src/hokuyo/shape-detect/offline.cc | 98 ++++++++++++++++++++++++- src/hokuyo/shape-detect/shape_detect.cc | 74 ++----------------- src/hokuyo/shape-detect/shape_detect.h | 49 +++---------- 3 files changed, 113 insertions(+), 108 deletions(-) diff --git a/src/hokuyo/shape-detect/offline.cc b/src/hokuyo/shape-detect/offline.cc index 639bd153..ce4804fc 100644 --- a/src/hokuyo/shape-detect/offline.cc +++ b/src/hokuyo/shape-detect/offline.cc @@ -18,6 +18,79 @@ #include +#include +using namespace std; + +ostream& operator << (ostream& os, Shape_detect::Point& point) +{ + os << "(" << point.x << ", " << point.y << ")"; + return os; +} + + +ostream& operator << (ostream& os, Shape_detect::Line& line) +{ + os << line.a << "--" << line.b; + return os; +} + +ostream& operator << (ostream& os, Shape_detect::Arc& arc) +{ + os << arc.center << " r=" << arc.radius; + return os; +} + + + +void gnuplot(std::vector &cartes, + std::vector &lines, + std::vector &arcs) +{ + FILE *gnuplot; + gnuplot = popen("gnuplot", "w"); + fprintf(gnuplot, "set grid\n"); + fprintf(gnuplot, "set nokey\n"); + fprintf(gnuplot, "set size ratio -1\n"); //1.3333 + fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n"); + fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n"); + fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n"); + fprintf(gnuplot, "set style fill transparent solid 0.2 noborder\n"); + + fprintf(gnuplot, "plot"); + if (cartes.size() > 0) + fprintf(gnuplot, "'-' with points ls 1"); + if (lines.size() > 0) + fprintf(gnuplot, ", '-' with linespoints ls 2"); + if (arcs.size() > 0) + fprintf(gnuplot, ", '-' with circles ls 2"); + fprintf(gnuplot, "\n"); + + + if (cartes.size() > 0) { + for (int i = 0; i < (int) cartes.size(); i++) { + fprintf(gnuplot, "%g %g\n", cartes[i].x, cartes[i].y); + } + fprintf(gnuplot, "e\n"); + } + + if (lines.size() > 0) { + for (int i = 0; i < (int) lines.size(); i++) { + fprintf(gnuplot, "%f %f\n%f %f\n\n", + lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y); + } + fprintf(gnuplot, "e\n"); + } + if (arcs.size() > 0) { + for (int i = 0; i < (int) arcs.size(); i++) { + fprintf(gnuplot, "%f %f %f\n", + arcs[i].center.x, arcs[i].center.y, arcs[i].radius); + } + fprintf(gnuplot, "e\n"); + } + + + pclose(gnuplot); +} /** * There are detected line segments in input file (laser scan data) @@ -33,9 +106,18 @@ int main(int argc, char** argv) return 1; } + char *fname = NULL; + bool plot = false; + + for (int i=1; i> number; + // Ignore out of range data (generated by simulator in robomon) + if (number >= 4000) + number = 0; laser_scan[idx] = number; idx++; } @@ -60,6 +145,17 @@ int main(int argc, char** argv) sd.line_detect(lines); sd.arc_detect(arcs); + for (unsigned i = 0; i < lines.size(); i++) { + cout << "Line: " << lines[i] << endl; + } + + for (unsigned i = 0; i < arcs.size(); i++) { + cout << "Arc: " << arcs[i] << endl; + } + + if (plot) + gnuplot(sd.getCartes(), lines, arcs); + return 0; } diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc index a670dfa5..3b2bc9ab 100644 --- a/src/hokuyo/shape-detect/shape_detect.cc +++ b/src/hokuyo/shape-detect/shape_detect.cc @@ -13,66 +13,6 @@ #include "shape_detect.h" -#ifdef GNUPLOT -void Shape_detect::plot_line(std::vector &lines) -{ - fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n"); - fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n"); - fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n"); - -#ifdef OFFLINE - fprintf(gnuplot, "plot"); -#else - fprintf(gnuplot, "plot [-1000:+3000] [-3000:+3000]"); -#endif - fprintf(gnuplot, "'-' with points ls 1, "); // points - fprintf(gnuplot, "'-' with linespoints ls 2,"); // lines - fprintf(gnuplot, "'-' with points ls 3"); //arc point - fprintf(gnuplot, "\n"); - - // points data - for (int i = 0; i < (int) cartes.size(); i++) { - fprintf(gnuplot, "%g %g\n", cartes[i].x, cartes[i].y); - } - fprintf(gnuplot, "e\n"); - - // lines data - for (int i = 0; i < (int) lines.size(); i++) { - fprintf(gnuplot, "%f %f\n%f %f\n\n", - lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y); - } - fprintf(gnuplot, "e\n"); -} - -void Shape_detect::plot_arc(std::vector &arcs) -{ - // arcs data - for (int i = 0; i < (int) arcs.size(); i++) { - fprintf(gnuplot, "%g %g\n", arcs[i].begin.x, arcs[i].begin.y); - fprintf(gnuplot, "%g %g\n", arcs[i].end.x, arcs[i].end.y); - } - fprintf(gnuplot, "e\n"); - - plot_close(); -} - -void Shape_detect::plot_close() -{ - fflush(gnuplot); - getchar(); - pclose(gnuplot); -} - -void Shape_detect::plot_init() -{ - gnuplot = popen("gnuplot", "w"); - fprintf(gnuplot, "set grid\n"); - fprintf(gnuplot, "set nokey\n"); -#ifdef OFFLINE - fprintf(gnuplot, "set size ratio 1\n"); //1.3333 -#endif -} -#endif // GNUPLOT Shape_detect::Shape_detect() { @@ -374,11 +314,13 @@ void Shape_detect::polar_to_cartes(const unsigned short laser_scan[]) } +std::vector &Shape_detect::getCartes() +{ + return cartes; +} + void Shape_detect::prepare(const unsigned short laser_scan[]) { -#ifdef GNUPLOT - plot_init(); -#endif polar_to_cartes(laser_scan); } @@ -398,9 +340,6 @@ void Shape_detect::arc_detect(std::vector &arcs) fit_arc(start, end, arcs); start = end + 1; } -#ifdef GNUPLOT - plot_arc(arcs); -#endif } void Shape_detect::line_detect(std::vector &lines) @@ -419,8 +358,5 @@ void Shape_detect::line_detect(std::vector &lines) line_fitting(start, end, lines); start = end + 1; } -#ifdef GNUPLOT - plot_line(lines); -#endif } diff --git a/src/hokuyo/shape-detect/shape_detect.h b/src/hokuyo/shape-detect/shape_detect.h index 89ac1f71..81d2810d 100644 --- a/src/hokuyo/shape-detect/shape_detect.h +++ b/src/hokuyo/shape-detect/shape_detect.h @@ -41,16 +41,14 @@ line_min_points. The founded line is written into output vector lines. The method line_detect is finished after research of all vectorparts. -\section shapedet_debug_mode Debug mode +\section shapedet_debug_mode Debugging -The debug mode permits detection of segment lines with connected laser scanner -or from scanned data, which are saved in a file. GNUPLOT can be used for graphical -output in debug mode or output vector can be saved in a file for next processing -in any application. - -For setting above properties is used directives define GNUPLOT. - -For debugging can be used methods introduced in file main.cc. +The debug mode permits detection of segment lines with connected laser +scanner or from scanned data, which are saved in a file. There is a +program @a shape_detect_offline, which takes the measured data +(generated for example by @a hokuyo-dump command) and writes out the +detected lines. It can also plot the results using gnuplot (-g +option). \section shapedet_example Example @@ -91,14 +89,6 @@ http://mathpages.com/home/kmath110.htm #include #include -/** - * Debug mode with GNUPLOT graph. - * True -> Detected lines are painted by GNUPLOT. - * False -> Lines isn't painted. - * @ingroup shapedet - */ -//#define GNUPLOT - /** * There are detected line segments in input array of measured data (laser_scan) * by using perpendicular line regression. @@ -168,6 +158,10 @@ class Shape_detect * @ingroup shapedet */ void prepare(const unsigned short laser_scan[]); + + /** Returns laser_scan data set by prepare converted to cartesian coordinates */ + std::vector &getCartes(); + /** * There are detected line segments in input array of measured @@ -210,27 +204,6 @@ class Shape_detect float Arc_min_aperture; float Arc_std_max; -#ifdef GNUPLOT - FILE *gnuplot; - - /** - * Method paints detection lines in GNUPLOT. - * For debug mode. It must be defined global flag GNUPLOT. - * @param &lines is vector which contains detected lines. - * @param &cartes is vector whose points are expressed in cartesian coordinates. - * @ingroup shapedet - */ - void plot_line(std::vector &lines); - void plot_arc(std::vector &arcs); - - /** - * Method for initialization GNUPLOT - opens pipe and performs basic settings. - * For debug mode. It must be defined global flag GNUPLOT. - * @ingroup shapedet - */ - void plot_init(); - void plot_close(); -#endif /** * Calculation of lines intersection. * @param point which pertains to line. -- 2.39.2