#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)
+void Shape_detect::plot_line(int begin, int end, int maxdist, std::vector<Shape_detect::Point> &cartes, std::vector<Shape_detect::Line> &lines)
{
fprintf(gnuplot, "set grid\n");
fprintf(gnuplot, "set nokey\n");
getchar();
}
-void Shape_detect::plot_shape_detect(std::vector<Shape_detect::Line> &lines, std::vector<Point> &cartes)
+void Shape_detect::plot_shape_detect(std::vector<Shape_detect::Line> &lines, std::vector<Shape_detect::Point> &cartes, std::vector<Shape_detect::Point> &arcs)
{
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");
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 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, "%g %g\n", cartes[i].x, cartes[i].y);
}
fprintf(gnuplot, "e\n");
lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
}
fprintf(gnuplot, "e\n");
+
+ // arcs data
+ for (int i = 0; i < (int) arcs.size(); i++) {
+ fprintf(gnuplot, "%g %g\n", arcs[i].x, arcs[i].y);
+ }
+ fprintf(gnuplot, "e\n");
fflush(gnuplot);
}
fprintf(gnuplot, "set grid\n");
fprintf(gnuplot, "set nokey\n");
#ifdef OFFLINE
- fprintf(gnuplot, "set size ratio 1.3333\n");
+ fprintf(gnuplot, "set size ratio 1\n"); //1.3333
#endif
}
#endif // GNUPLOT
Shape_detect::Line_min_points = 7;
Shape_detect::Line_error_threshold = 20;
Shape_detect::Max_distance_point = 300;
+
+ Shape_detect::Arc_std_max = 0.15; //0.15
+ Shape_detect::Arc_min_aperture = 1.57; //90 degrees
+ Shape_detect::Arc_max_aperture = 2.5; //2.365 #135 degrees
}
Shape_detect::Shape_detect(int line_min_points, int line_error_threshold, int max_distance_point)
return tmp;
}
+inline Shape_detect::Point Shape_detect::rotate(Shape_detect::Point input_point, float rad)
+{
+ Shape_detect::Point tmp;
+ tmp.x = input_point.x * cos(rad) - input_point.y * sin(rad);
+ tmp.y = input_point.x * sin(rad) + input_point.y * cos(rad);
+
+ return tmp;
+}
+
+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));
+}
+
+bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Point> &cartes, std::vector<Shape_detect::Point> &arcs)
+{
+ std::cout << "rozsah: " << begin << " - " << end << " -> ";
+
+ if (end-begin < 4) {
+ std::cout << "mensi nez 4" << std::endl;
+ return false;
+ }
+
+ int idx_middle_point = (end + begin) / 2;
+ Point rotated_point = cartes[idx_middle_point];
+ Point right = cartes[begin];
+ Point left = cartes[end];
+ rotated_point.x = rotated_point.x - left.x;
+ rotated_point.y = rotated_point.y - left.y;
+ float angle_to_rotate = atan2((left.x - right.x) / (left.y - right.y),1);
+ rotated_point = rotate(rotated_point, angle_to_rotate);
+
+ //std::cout << "rotacni bod: " << rotated_point.x << ", " << rotated_point.y << "; vzdalenost: " << Shape_detect::point_distance(left, right) << "; ";
+
+ if (-rotated_point.x > .1 * Shape_detect::point_distance(left, right) &&
+ -rotated_point.x < Shape_detect::point_distance(left, right)) {
+ if (end - begin > 20) {
+ end--;
+ begin++;
+ right = cartes[begin];
+ left = cartes[end];
+ }
+ } else {
+ std::cout << "rotace bodu" << std::endl;
+ return false;
+ }
+
+ int segment_size = end - begin + 1;
+ int slopes_size = segment_size - 3;
+
+ float ma, mb, slopes[slopes_size];
+
+ for (int i = 0; i < slopes_size; i++) {
+ Shape_detect::Point tmp = cartes[begin + i + 1];
+ ma = atan2(left.y - tmp.y, left.x - tmp.x);
+ mb = atan2(right.y - tmp.y, right.x - tmp.x);
+
+ slopes[i] = ma - mb;
+ }
+
+ if (ma == 0)
+ ma = 1e-8;
+ if (mb == 0)
+ mb = 1e-8;
+
+ float sum = 0;
+ for (int i = 0; i < slopes_size; i++)
+ sum = sum + slopes[i];
+
+ float average = sum / slopes_size;
+
+ float totalvar = 0;
+ int count = slopes_size;
+
+ while (count > 0) {
+ count--;
+ totalvar = totalvar + ((slopes[count] - average) * (slopes[count] - average));
+ }
+
+ float standard_deviation = sqrt(totalvar / --slopes_size);
+
+ if (standard_deviation < Arc_std_max && Arc_max_aperture > average && average > Arc_min_aperture) {
+
+ std::cout << "std: " << standard_deviation << " => ";
+
+ Shape_detect::Point tmp;
+ tmp.x = right.x - left.x;
+ tmp.y = right.y - left.y;
+
+ tmp = Shape_detect::rotate(tmp, -angle_to_rotate);
+
+ float middle = tmp.x / 2.0;
+ float q = average - M_PI / 2.0;
+ float height = middle * tan(q);
+
+ Shape_detect::Point center;
+ center.x = middle;
+ center.y = height;
+
+ //float radius = float(sqrt(center.y * center.y + center.x * center.x));
+
+ center = Shape_detect::rotate(center, angle_to_rotate);
+ center.x = center.x + left.x;
+ center.y = center.y + left.y;
+
+ arcs.push_back(cartes[begin]);
+ arcs.push_back(cartes[end]);
+ std::cout << "Detekovan" << std::endl;
+ return 1;
+ }
+
+ fit_arc(begin + 1, end - 1, cartes, arcs);
+
+ std::cout << "Nic" << std::endl;
+ return 0;
+}
+
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;
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 b1 = med_y - m1*med_x;
float b2 = med_y - m2*med_x;
-
+
// maximum error
r = 0;
unsigned ir = -1;
// 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;
ir = i;
}
}
-
+
if (r < r1) {
gen.a = m1;
gen.c = b1;
return 0;
}
-// line recursive fitting
+ // 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) < Shape_detect::Line_min_points) return;
} 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 fi;
int r;
- for (int i = 0; i < (int) HOKUYO_ARRAY_SIZE - 1; i++) {
+ for (int i = 0; i < (int) HOKUYO_ARRAY_SIZE; i++) {
r = (laser_scan[i] <= 19) ? 0 : laser_scan[i];
if (r > 0) {
}
}
-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 unsigned short laser_scan[], std::vector<Shape_detect::Line> &lines)
{
#ifdef GNUPLOT
// polar coordinates to cartesian coordinates
std::vector<Shape_detect::Point> cartes;
- std::cout << laser_scan[50] << std::endl;
-
polar_to_cartes(laser_scan, cartes);
int cartes_size = cartes.size();
- std::cout << "size " << cartes_size << std::endl;
+ std::vector<Shape_detect::Point> arcs;
+ std::cout << "cartes: " << cartes_size << std::endl;
+
+ int end, start = 0;
+ while (start < cartes_size) {
+ end = start + 1;
+
+ while (point_distance(cartes[end-1], cartes[end]) < 100 && end < cartes_size)
+ end++;
+
+ end --;
+
+ fit_arc(start, end, cartes, arcs);
+ start = end + 1;
+ }
+/*
int end, start = 0;
while (start < cartes_size) {
end = start + 1;
line_fitting(start, end, cartes, lines);
start = end + 1;
}
+*/
#ifdef GNUPLOT
- plot_shape_detect(lines, cartes);
+ plot_shape_detect(lines, cartes, arcs);
getchar();
pclose(gnuplot);
#endif
projects / eurobot / public.git / blobdiff