-#include <string>
-#include <cstring>
-#include <stdlib.h>
-#include <fstream>
-#include <iostream>
-#include <sstream>
-#include <math.h>
-#include <vector>
-#include <hokuyo.h>
-#include <robot.h>
-#include <robomath.h>
-#include <robottype.h>
-#include <roboorte_robottype.h>
-#define OFFLINE 1
-#define LINE_MIN_POINTS 7
-#define LINE_ERROR_THRESHOLD 20
-#define MAX_DISTANCE_POINT 300
-
-using namespace std;
-
-// Line equation - General form
-typedef struct {float a,b,c;} General_form;
-
-typedef struct {float x,y;} Point;
-
-typedef struct {Point a,b;} Line;
-
-FILE *gnuplot;
-
-static inline Point intersection_line(const Point point, const General_form gen) {
- Point tmp;
-
- tmp.x = (gen.b*gen.b*point.x - gen.a*gen.b*point.y - gen.a*gen.c) / (gen.a*gen.a + gen.b*gen.b);
- tmp.y = (gen.b*tmp.x - gen.b*point.x + gen.a*point.y) / gen.a;
-
- return tmp;
-}
-
-int perpendicular_regression(float &r, int &max_diff_idx, const int begin, const int end, vector<Point> &cartes, General_form &gen)
-{
- int number_points = abs(end-begin) + 1;
-
- if (number_points <= 0) return 1;
-
- float sum_x = 0;
- float sum_y = 0;
-
- for (int i = begin; i <= end; i++) {
- sum_x = sum_x + cartes[i].x;
- sum_y = sum_y + cartes[i].y;
- }
-
- float med_x = sum_x / number_points;
- float med_y = sum_y / number_points;
-
- vector<Point> point_average(number_points);
-
- Point tmp;
-
- int j = 0;
-
- for (int i = begin; i <= end; i++) {
- tmp.x = cartes[i].x - med_x;
- tmp.y = cartes[i].y - med_y;
- point_average[j] = tmp;
- j++;
- }
-
- float A = 0;
- float sum_xy = 0;
-
- for (int i = 0; i < number_points; i++) {
- A = A + (point_average[i].x*point_average[i].x - point_average[i].y*point_average[i].y);
- sum_xy = sum_xy + point_average[i].x * point_average[i].y;
- }
-
- if (sum_xy == 0) sum_xy = 1e-8;
-
- A = A / sum_xy;
-
- // tan(q)^2 + A*tan(q) - 1 = 0 ( tan(q) sign as m ) -> quadratic equation
- float m1 = (-A + sqrt(A*A + 4)) / 2;
- float m2 = (-A - sqrt(A*A + 4)) / 2;
-
- float b1 = med_y - m1*med_x;
- float b2 = med_y - m2*med_x;
-
- // maximum error
- r = 0;
- unsigned ir = -1;
- float dist;
-
- float r1 = 0;
- unsigned ir1 = -1;
- float dist1;
-
- for (int i = begin; i < end; i++) {
- // distance point from the line (A = m1, B = -1, C = b1)
- dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) );
- dist1 = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
-
- if (dist1 > r1) {
- r1 = dist1;
- ir1 = i;
- }
-
- if (dist > r) {
- r = dist;
- ir = i;
- }
- }
-
- if (r < r1) {
- max_diff_idx = ir;
- gen.a = m1;
- gen.c = b1;
- } else {
- r = r1;
- max_diff_idx = ir1;
- gen.a = m2;
- gen.c = b2;
- }
-
- gen.b = -1.0;
-
- return 0;
-}
-
-void plot_line(int begin, int end, int maxdist, vector<Point> &cartes, vector<Line> &lines)
-{
- fprintf(gnuplot, "set grid\n");
- fprintf(gnuplot, "set nokey\n");
- fprintf(gnuplot, "set style line 1 lt 2 lc rgb \"red\" lw 3\n");
- fprintf(gnuplot, "plot 'cartes1' with points ls 2, 'cartes2' with points ls 3");
-
- fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 4 with linespoints",cartes[begin].x, cartes[begin].y, cartes[end].x, cartes[end].y);
- fprintf(gnuplot, ", \"< echo \'%f %f\'\" ls 1 pointsize 3 with points",cartes[maxdist].x, cartes[maxdist].y);
-
- for (int i = 0; i < (int) lines.size(); i++) {
- fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 1 with linespoints",lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
- }
-
- fprintf(gnuplot, "\n");
- fflush(gnuplot);
- getchar();
-}
-
-// line recursive fitting
-void line_fitting(int begin, int end, vector<Point> &cartes, vector<Line> &lines) {
- int line_break_point;
- //cout << "begin: " << begin << " end: " << end << endl << flush;
-
- if ((end - begin) < LINE_MIN_POINTS) return;
-
- float r;
- General_form gen;
-
- if (perpendicular_regression(r, line_break_point, begin, end, cartes, gen)) return; // r = 0
-
- if (r < LINE_ERROR_THRESHOLD) {
- Line tmp;
-
- tmp.a = intersection_line(cartes[begin], gen);
- tmp.b = intersection_line(cartes[end], gen);
-
- lines.push_back(tmp);
-
- //cout << endl << "begin X: " << cartes[begin].x << " Y: " << cartes[begin].y << endl << flush;
- //cout << "end X: " << cartes[end].x << " Y: " << cartes[end].y << endl << endl << flush;
- } else {
- // Ax+By+C=0
- // normal vector: n[n_x, -n_y]
-#if 1
- float n_x = cartes[begin].y - cartes[end].y;
- float n_y = cartes[begin].x - cartes[end].x;
-
- float A = n_x;
- float B = -n_y;
- float C = n_y*cartes[end].y - n_x*cartes[end].x;
-
- int line_break_point = 0;
- float dist, dist_max = 0;
+#include <shape_detect.h>
- for (int i = begin; i < end; i++) {
- // distance point from the line
- dist = fabs( (cartes[i].x*A + cartes[i].y*B + C) / sqrt(A*A + B*B));
-
- if (dist > dist_max) {
- dist_max = dist;
- line_break_point = i;
- }
- }
-
- //plot_line(begin, end, line_break_point, cartes, lines);
-
- if (dist_max > LINE_ERROR_THRESHOLD) {
- line_fitting(begin, line_break_point, cartes, lines);
- line_fitting(line_break_point, end, cartes, lines);
- }
-#else
- if (line_break_point == begin)
- line_break_point++;
- else if (line_break_point == end)
- line_break_point--;
-
- line_fitting(begin, line_break_point, cartes, lines);
- line_fitting(line_break_point, end, cartes, lines);
-#endif
- } // end if (r <= LINE_ERROR_THRESHOLD)
-
- return;
-}
-
-// polar to cartesian coordinates
-void polar_to_cartes(const vector<int> &input_data, vector<Point> &cartes) {
- Point point;
-
- float fi;
- int r;
-
- for (int i = 0; i < (int) input_data.size()-1; i++) {
- r = (input_data[i] <= 19) ? 0 : input_data[i];
-
- if (r > 0) {
- fi = HOKUYO_INDEX_TO_RAD(i);
- point.x = r * cos(fi);
- point.y = r * sin(fi);
- cartes.push_back(point);
- }
- }
-}
-
-// save vector to file
-/*
-void file_save(const vector<Point> &data, string name) {
- char * file_name;
- file_name = new char[name.length() + 1];
- strcpy(file_name, name.c_str());
-
- FILE *file = fopen(file_name,"w");
-
- for (int i = 0; i < (int) data.size(); i++)
- fprintf(file, "%f, %f\n", data[i].x, data[i].y);
-
- fclose(file);
- delete file_name;
-}
-*/
-static inline float point_distance(Point a, Point b) {
- return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
-}
-
-void shape_detect(const vector<int> &input_data, vector<Line> &lines) {
- // polar coordinates to cartesian coordinates
- vector<Point> cartes;
- polar_to_cartes(input_data, cartes);
-
- int cartes_size = cartes.size();
-
- int end, start = 0;
- while (start < cartes_size) {
- end = start + 1;
-
- while (point_distance(cartes[end-1], cartes[end]) < MAX_DISTANCE_POINT && end < cartes_size)
- end++;
-
- end--;
-
- line_fitting(start, end, cartes, lines);
- start = end + 1;
- }
-
-
- 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");
-
-#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, "\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");
- fflush(gnuplot);
-}
+// connect Hokuyo
+#define OFFLINE 1
-static void gnuplot_init()
+#ifdef OFFLINE
+int main(int argc, char** argv)
{
- gnuplot = popen("gnuplot", "w");
- fprintf(gnuplot, "set grid\n");
- fprintf(gnuplot, "set nokey\n");
-#ifndef OFFLINE
- fprintf(gnuplot, "set size ratio 1.3333\n");
-#endif
-}
+ Shape_detect sd;
-#ifdef OFFLINE
-int main(int argc, char** argv) {
-
if (argc < 2) {
- cout << "Error: Invalid number of input parameters." << endl;
+ std::cout << "Error: Invalid number of input parameters." << std::endl;
return 1;
}
- vector<int> input_data;
+ std::vector<int> input_data;
- ifstream infile(argv[1], ios_base::in);
+ std::ifstream infile(argv[1], std::ios_base::in);
// line input file
- string line;
+ std::string line;
// number from input file
int number;
int tmp = 1;
- while (getline(infile, line, ',')) {
+ while (std::getline(infile, line, ',')) {
if (tmp) {
tmp = 0;
continue;
}
if (line != "\n") {
- stringstream strStream(line);
+ std::stringstream strStream(line);
strStream >> number;
input_data.push_back(number);
tmp = 1;
}
}
- vector<Line> output_data;
-
- gnuplot_init();
-
+ std::vector<Shape_detect::Line> output_data;
// detect line
- shape_detect(input_data, output_data);
-
- getchar();
-
+ sd.shape_detect(input_data, output_data);
return 0;
}
+
#else
struct robottype_orte_data orte;
void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
+ Shape_detect sd;
+
struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
static int count = 0;
printf("Detect\n");
count = 0;
- vector<int> input(HOKUYO_ARRAY_SIZE);
+ std::vector<int> input(HOKUYO_ARRAY_SIZE);
for(unsigned i = 0; i < HOKUYO_ARRAY_SIZE; i++)
input[i] = (int) instance->data[i];
- vector<Line> output;
- shape_detect(input, output);
+ std::vector<Shape_detect::Line> output;
+ sd.shape_detect(input, output);
}
break;
}
int main()
{
- gnuplot_init();
robot_init_orte();
- getchar();
- pclose(gnuplot);
return 0;
}
+
#endif
+
--- /dev/null
+#include "shape_detect.h"
+
+#ifdef GNUPLOT
+void Shape_detect::plot_line(int begin, int end, int maxdist, std::vector<Point> &cartes, std::vector<Shape_detect::Line> &lines)
+{
+ fprintf(gnuplot, "set grid\n");
+ fprintf(gnuplot, "set nokey\n");
+ fprintf(gnuplot, "set style line 1 lt 2 lc rgb \"red\" lw 3\n");
+ fprintf(gnuplot, "plot 'cartes1' with points ls 2, 'cartes2' with points ls 3");
+
+ fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 4 with linespoints",cartes[begin].x, cartes[begin].y, cartes[end].x, cartes[end].y);
+ fprintf(gnuplot, ", \"< echo \'%f %f\'\" ls 1 pointsize 3 with points",cartes[maxdist].x, cartes[maxdist].y);
+
+ for (int i = 0; i < (int) lines.size(); i++) {
+ fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 1 with linespoints",lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
+ }
+
+ fprintf(gnuplot, "\n");
+ fflush(gnuplot);
+ getchar();
+}
+
+void Shape_detect::plot_shape_detect(std::vector<Shape_detect::Line> &lines, std::vector<Point> &cartes)
+{
+ 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");
+
+#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, "\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");
+ fflush(gnuplot);
+}
+
+void Shape_detect::gnuplot_init()
+{
+ gnuplot = popen("gnuplot", "w");
+ fprintf(gnuplot, "set grid\n");
+ fprintf(gnuplot, "set nokey\n");
+#ifdef OFFLINE
+ fprintf(gnuplot, "set size ratio 1.3333\n");
+#endif
+}
+#endif // GNUPLOT
+
+Shape_detect::Shape_detect (void) {};
+
+inline Shape_detect::Point Shape_detect::intersection_line(const Shape_detect::Point point, const General_form gen)
+{
+ Shape_detect::Point tmp;
+
+ tmp.x = (gen.b*gen.b*point.x - gen.a*gen.b*point.y - gen.a*gen.c) / (gen.a*gen.a + gen.b*gen.b);
+ tmp.y = (gen.b*tmp.x - gen.b*point.x + gen.a*point.y) / gen.a;
+
+ return tmp;
+}
+
+int Shape_detect::perpendicular_regression(float &r, const int begin, const int end, std::vector<Shape_detect::Point> &cartes, General_form &gen)
+{
+ int number_points = abs(end-begin) + 1;
+
+ if (number_points <= 0) return 1;
+
+ float sum_x = 0;
+ float sum_y = 0;
+
+ for (int i = begin; i <= end; i++) {
+ sum_x = sum_x + cartes[i].x;
+ sum_y = sum_y + cartes[i].y;
+ }
+
+ float med_x = sum_x / number_points;
+ float med_y = sum_y / number_points;
+
+ Shape_detect::Point tmp;
+
+ float A = 0;
+ float sum_xy = 0;
+
+ for (int i = begin; i <= end; i++) {
+ tmp.x = cartes[i].x - med_x;
+ tmp.y = cartes[i].y - med_y;
+ A = A + (tmp.x*tmp.x - tmp.y*tmp.y);
+ sum_xy = sum_xy + tmp.x * tmp.y;
+ }
+
+ if (sum_xy == 0) sum_xy = 1e-8;
+
+ A = A / sum_xy;
+
+ // tan(q)^2 + A*tan(q) - 1 = 0 ( tan(q) sign as m ) -> quadratic equation
+ float m1 = (-A + sqrt(A*A + 4)) / 2;
+ float m2 = (-A - sqrt(A*A + 4)) / 2;
+
+ float b1 = med_y - m1*med_x;
+ float b2 = med_y - m2*med_x;
+
+ // maximum error
+ r = 0;
+ unsigned ir = -1;
+ float dist;
+
+ float r1 = 0;
+ unsigned ir1 = -1;
+ float dist1;
+
+ for (int i = begin; i < end; i++) {
+ // distance point from the line (A = m1, B = -1, C = b1)
+ dist = fabs( (cartes[i].x*m1 - cartes[i].y + b1) / sqrt(m1*m1 + 1) );
+ dist1 = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
+
+ if (dist1 > r1) {
+ r1 = dist1;
+ ir1 = i;
+ }
+
+ if (dist > r) {
+ r = dist;
+ ir = i;
+ }
+ }
+
+ if (r < r1) {
+ gen.a = m1;
+ gen.c = b1;
+ } else {
+ r = r1;
+ gen.a = m2;
+ gen.c = b2;
+ }
+
+ gen.b = -1.0;
+
+ return 0;
+}
+
+// line recursive fitting
+void Shape_detect::line_fitting(int begin, int end, std::vector<Shape_detect::Point> &cartes, std::vector<Shape_detect::Line> &lines)
+{
+ if ((end - begin) < LINE_MIN_POINTS) return;
+
+ float r;
+ General_form gen;
+
+ if (perpendicular_regression(r, begin, end, cartes, gen)) return; // r = 0
+
+ if (r < LINE_ERROR_THRESHOLD) {
+ Shape_detect::Line tmp;
+
+ tmp.a = intersection_line(cartes[begin], gen);
+ tmp.b = intersection_line(cartes[end], gen);
+
+ lines.push_back(tmp);
+ } else {
+ // Ax+By+C=0
+ // normal vector: n[n_x, -n_y]
+
+ float n_x = cartes[begin].y - cartes[end].y;
+ float n_y = cartes[begin].x - cartes[end].x;
+
+ float A = n_x;
+ float B = -n_y;
+ float C = n_y*cartes[end].y - n_x*cartes[end].x;
+
+ int line_break_point = 0;
+ float dist, dist_max = 0;
+
+ for (int i = begin; i < end; i++) {
+ // distance point from the line
+ dist = fabs( (cartes[i].x*A + cartes[i].y*B + C) / sqrt(A*A + B*B));
+
+ if (dist > dist_max) {
+ dist_max = dist;
+ line_break_point = i;
+ }
+ }
+
+ //plot_line(begin, end, line_break_point, cartes, lines);
+
+ if (dist_max > LINE_ERROR_THRESHOLD) {
+ line_fitting(begin, line_break_point, cartes, lines);
+ line_fitting(line_break_point, end, cartes, lines);
+ }
+
+ } // end if (r <= LINE_ERROR_THRESHOLD)
+
+ return;
+}
+
+// polar to cartesian coordinates
+void Shape_detect::polar_to_cartes(const std::vector<int> &input_data, std::vector<Shape_detect::Point> &cartes)
+{
+ Shape_detect::Point point;
+
+ float fi;
+ int r;
+
+ for (int i = 0; i < (int) input_data.size()-1; i++) {
+ r = (input_data[i] <= 19) ? 0 : input_data[i];
+
+ if (r > 0) {
+ fi = HOKUYO_INDEX_TO_RAD(i);
+ point.x = r * cos(fi);
+ point.y = r * sin(fi);
+ cartes.push_back(point);
+ }
+ }
+}
+
+inline float Shape_detect::point_distance(Shape_detect::Point a, Shape_detect::Point b)
+{
+ return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
+}
+
+void Shape_detect::shape_detect(const std::vector<int> &input_data, std::vector<Shape_detect::Line> &lines)
+{
+#ifdef GNUPLOT
+ gnuplot_init();
+#endif
+ // polar coordinates to cartesian coordinates
+ std::vector<Shape_detect::Point> cartes;
+ polar_to_cartes(input_data, cartes);
+
+ int cartes_size = cartes.size();
+
+ int end, start = 0;
+ while (start < cartes_size) {
+ end = start + 1;
+
+ while (point_distance(cartes[end-1], cartes[end]) < MAX_DISTANCE_POINT && end < cartes_size)
+ end++;
+
+ end--;
+
+ line_fitting(start, end, cartes, lines);
+ start = end + 1;
+ }
+#ifdef GNUPLOT
+ plot_shape_detect(lines, cartes);
+ getchar();
+ pclose(gnuplot);
+#endif
+}
+
projects / eurobot / public.git / commitdiff