#include <string>
+#include <cstring>
#include <stdlib.h>
#include <fstream>
#include <iostream>
#include <math.h>
#include <vector>
#include <hokuyo.h>
+#include <robot.h>
#include <robomath.h>
+#include <robottype.h>
+#include <roboorte_robottype.h>
#define LINE_MIN_POINTS 7
-#define LINE_ERROR_THRESHOLD 25
+#define LINE_ERROR_THRESHOLD 20
+#define MAX_DISTANCE_POINT 300
using namespace std;
typedef struct {float x,y;} Point;
-vector<Point> cartes;
-vector<Point> lines;
+typedef struct {Point a,b;} Line;
-int perpendicular_regression(float &r, int &max_diff_idx, const int begin, const int end)
+int perpendicular_regression(float &r, int &max_diff_idx, const int begin, const int end, vector<Point> &cartes)
{
int number_points = abs(end-begin) + 1;
float med_x = sum_x / number_points;
float med_y = sum_y / number_points;
- vector<Point> point_average;
-
+ 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.push_back(tmp);
+ point_average[j] = tmp;
+ j++;
}
float A = 0;
// 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 m2 = (-A - sqrt(A*A + 4)) / 2;
float b1 = med_y - m1*med_x;
float b2 = med_y - m2*med_x;
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;
}
}
- float r1 = 0;
- unsigned ir1 = -1;
-
- for (int i = begin; i < end; i++) {
- // distance point from the line (A = m2, B = -1, C = b2)
- dist = fabs( (cartes[i].x*m2 - cartes[i].y + b2) / sqrt(m2*m2 + 1) );
- if (dist > r1) {
- r1 = dist;
- ir1 = i;
- }
- }
-
- if (r < r1)
- max_diff_idx = ir;
- else {
- r = r1;
+ if (r < r1) {
max_diff_idx = ir1;
+ } else {
+ r = r1;
+ max_diff_idx = ir;
}
return 0;
}
// line recursive fitting
-void line_fitting(int begin, int end) {
+void line_fitting(int begin, int end, vector<Point> &cartes, vector<Line> &lines) {
int line_break_point;
- cout << "begin: " << begin << " end: " << end << endl << flush;
+ //cout << "begin: " << begin << " end: " << end << endl << flush;
if ((end - begin) < LINE_MIN_POINTS) return;
float r;
- if (perpendicular_regression(r, line_break_point, begin, end)) return; // r = 0
+ if (perpendicular_regression(r, line_break_point, begin, end, cartes)) return; // r = 0
if (r < LINE_ERROR_THRESHOLD) {
- lines.push_back(cartes[begin]);
- lines.push_back(cartes[end]);
- 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;
+ Line line;
+
+ line.a = cartes[begin];
+ line.b = cartes[end];
+
+ lines.push_back(line);
+
+ //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]
+ 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;
+ }
+ }
+
+ if (dist_max > LINE_ERROR_THRESHOLD) {
+ line_fitting(begin, line_break_point, cartes, lines);
+ line_fitting(line_break_point, end, cartes, lines);
+ }
+ /*
if (line_break_point == begin)
line_break_point++;
else if (line_break_point == end)
line_break_point--;
- line_fitting(begin, line_break_point);
- line_fitting(line_break_point, end);
+
+ line_fitting(begin, line_break_point, cartes, lines);
+ line_fitting(line_break_point, end, cartes, lines);
+ */
} // end if (r <= LINE_ERROR_THRESHOLD)
-
- return;
+ return;
}
// polar to cartesian coordinates
-void polar_to_cartes(const vector<int> &input_data) {
-
+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 += 2) {
- r = (input_data[i+1] <= 19) ? 0 : input_data[i+1];
- //fi = HOKUYO_INDEX_TO_RAD(RESOLUTION * input_data[i] - 45) * PI/180;
+ 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(input_data[i]);
-
+ fi = HOKUYO_INDEX_TO_RAD(i);
point.x = r * cos(fi);
point.y = r * sin(fi);
cartes.push_back(point);
}
}
-void shape_detect(const vector<int> &input_data) {
+// 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;
+}
+
+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
- polar_to_cartes(input_data);
- cout << "Cartes vector size: " << cartes.size() << endl << endl;
-
- int start = 0;
- while (..) {
- i = start + 1;
- while (distance(cartes[i-1], cartes[i]) < 50cm &&
- i < cartes.size())
- i++;
-
- // detection lines
- line_fitting(start, i);
- start = i+1;
+ 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++;
+
+ if(end == cartes_size) end--;
+
+ line_fitting(start, end, cartes, lines);
+ start = end + 1;
+ }
+
+ // cartes -> save file
+ file_save(cartes, "cartes_data");
+
+ // create pipe -> gnuplot
+ FILE *pipe = popen("gnuplot", "w");
+
+ fprintf(pipe, "set grid\n");
+ fprintf(pipe, "set nokey\n");
+ fprintf(pipe, "set style line 1 lt 2 lc rgb \"green\" lw 3\n");
+ fprintf(pipe, "plot \"cartes_data\" with points");
+
+ for (int i = 0; i < (int) lines.size(); i++) {
+ fprintf(pipe, ", \"< 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(pipe, "\n");
+ fflush(pipe);
+
+ getchar();
+ pclose(pipe);
}
+//-----------------------------------------------
+/*
struct robottype_orte_data orte;
void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
switch (info->status) {
case NEW_DATA: {
scan = *instance;
- scan.data[i]
- shape_detect(scan);
- save_data(lines);
- system("gnuplot graph.dem"); // nebo pres pipe jako v barcam
+
+
+ vector<int> input(velikost);
+
+ for(int i = 0; i < velikost; i++)
+ input[i] = (int) scan.data[i];
+
+
+
+ vector<Line> output;
+ shape_detect(input, output);
+
break;
}
case DEADLINE:
robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
//system("killall -9 hokuyo");
- DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
+ //DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
break;
}
}
+
int robot_init_orte()
{
int rv = 0;
-
robot.orte.strength = 20;
rv = robottype_roboorte_init(&orte);
- if (rv)
- return rv;
+ if (rv) return rv;
robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
-
return rv;
}
-
+*/
+//-----------------------------------------------
// MAIN
int main(int argc, char** argv) {
- if (argc < 2) {
- cout << "Error: Invalid number of input parameters." << endl;
- return 1;
- }
-
- vector<int> input_data;
-
- ifstream infile (argv[1], ios_base::in);
-
- string line; // line input file
- int number; // number from input file
-
- while (getline(infile, line, ',')) {
- if (line != "\n") {
- stringstream strStream(line);
- strStream >> number;
- input_data.push_back(number);
- }
- }
-
- // detect line
- shape_detect(input_data);
-
- // save file
- FILE *file = fopen("output_data","w");
+ if (argc < 2) {
+ cout << "Error: Invalid number of input parameters." << endl;
+ return 1;
+ }
- for (int i = 0; i < (int) lines.size(); i++)
- fprintf(file, "%f, %f\n", lines[i].x, lines[i].y);
+ vector<int> input_data;
+
+ ifstream infile(argv[1], ios_base::in);
- fclose(file);
+ // line input file
+ string line;
- file = fopen("cartes_data","w");
+ // number from input file
+ int number;
- for (int i = 0; i < (int) cartes.size(); i++)
- fprintf(file, "%f, %f\n", cartes[i].x, cartes[i].y);
+ int tmp = 1;
+
+ while (getline(infile, line, ',')) {
+ if (tmp) {
+ tmp = 0;
+ continue;
+ }
+ if (line != "\n") {
+ stringstream strStream(line);
+ strStream >> number;
+ input_data.push_back(number);
+ tmp = 1;
+ }
+ }
- fclose(file);
-
- return 0;
+ vector<Line> output_data;
+
+ // detect line
+ shape_detect(input_data, output_data);
+
+ return 0;
}
projects / eurobot / public.git / commitdiff