From 0076c52ddb9349d39c51f28bc74aba3fc8f9817d Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Sat, 1 Dec 2012 11:17:06 +0100 Subject: malzeugs ausgelagert --- detect_road_borders.cpp | 89 ++++++++++++++++++++++++++----------------------- 1 file changed, 48 insertions(+), 41 deletions(-) diff --git a/detect_road_borders.cpp b/detect_road_borders.cpp index 38dabdc..14841f3 100644 --- a/detect_road_borders.cpp +++ b/detect_road_borders.cpp @@ -428,7 +428,7 @@ int find_bestquality_index(const vector& contour, double* angle_derivativ *bestquality_width_out = bestquality_width; } -int find_ideal_line(int xlen, int ylen, vector& contour, int** contour_map, int bestquality_j, Mat drawing) +int find_ideal_line(int xlen, int ylen, vector& contour, int** contour_map, int bestquality_j) // TODO: this code is crappy, slow, and uses brute force. did i mention it's crappy and slow? { int intersection = find_intersection_index(xlen/2, ylen-ylen/5, @@ -442,9 +442,6 @@ int find_ideal_line(int xlen, int ylen, vector& contour, int** contour_ma } else { - circle(drawing, contour[intersection], 2, Scalar(0,0,0)); - circle(drawing, contour[intersection], 1, Scalar(0,0,0)); - int xx=contour[bestquality_j].x; int lastheight=-1; if (intersection < bestquality_j) // too far on the right == intersecting the right border @@ -493,35 +490,14 @@ int find_ideal_line(int xlen, int ylen, vector& contour, int** contour_ma } -#define SMOOTHEN_BOTTOM 25 -#define SMOOTHEN_MIDDLE 10 -#define ANG_SMOOTH 9 -// return the index of the point to steer to. -int find_steering_point(Mat orig_img, int** contour_map, Mat& drawing) // orig_img is a binary image +void draw_it_all(Mat drawing, vector< vector >& contours, const vector& hierarchy, int first_nonbottom_idx, vector& contour, + double* angles, double* angle_derivative, int bestquality_j, int bestquality_width, int bestquality, + int steering_point, Point origin_point) { - Mat img; - orig_img.copyTo(img); // this is needed because findContours destroys its input. - - vector > contours; - vector hierarchy; - - findContours(img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE, Point(0, 0)); - - int low_y, low_idx, high_y, first_nonbottom_idx; - vector& contour = prepare_and_get_contour(img.cols, img.rows, contours, hierarchy, - &low_y, &low_idx, &high_y, &first_nonbottom_idx); - - init_contour_map(contour, contour_map, img.cols, img.rows); - // Draw contours - drawing = Mat::zeros( img.size(), CV_8UC3 ); drawContours(drawing, contours, -1, Scalar(255,0,0), 1, 8, hierarchy); - - double* angles = calc_contour_angles(contour, first_nonbottom_idx, img.rows, SMOOTHEN_MIDDLE, SMOOTHEN_BOTTOM); - double* angle_derivative = calc_angle_deriv(angles, first_nonbottom_idx, contour.size(), ANG_SMOOTH); - - // irrelevant drawing stuff + // draw the angles for (int j=first_nonbottom_idx; j=0) // should be always true - line(drawing, contour[steering_point], Point(img.cols/2, img.rows-img.rows/5), Scalar(0,255,255)); + line(drawing, contour[steering_point], origin_point, Scalar(0,255,255)); +} + +#define SMOOTHEN_BOTTOM 25 +#define SMOOTHEN_MIDDLE 10 +#define ANG_SMOOTH 9 +// return the index of the point to steer to. +int find_steering_point(Mat orig_img, int** contour_map, Mat& drawing) // orig_img is a binary image +{ + Mat img; + orig_img.copyTo(img); // this is needed because findContours destroys its input. + drawing = Mat::zeros( img.size(), CV_8UC3 ); + + vector > contours; + vector hierarchy; + + findContours(img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE, Point(0, 0)); + + int low_y, low_idx, high_y, first_nonbottom_idx; + vector& contour = prepare_and_get_contour(img.cols, img.rows, contours, hierarchy, + &low_y, &low_idx, &high_y, &first_nonbottom_idx); + + init_contour_map(contour, contour_map, img.cols, img.rows); + + + double* angles = calc_contour_angles(contour, first_nonbottom_idx, img.rows, SMOOTHEN_MIDDLE, SMOOTHEN_BOTTOM); + double* angle_derivative = calc_angle_deriv(angles, first_nonbottom_idx, contour.size(), ANG_SMOOTH); + + int bestquality, bestquality_j, bestquality_width; + find_bestquality_index(contour, angle_derivative, img.cols, img.rows, high_y, first_nonbottom_idx, drawing, + &bestquality_j, &bestquality_width, &bestquality); + + // now we have a naive steering point. the way to it might lead + // us offroad, however. + + int steering_point=find_ideal_line(img.cols,img.rows, contour, contour_map, bestquality_j); + + draw_it_all(drawing, contours, hierarchy, first_nonbottom_idx, contour, angles, angle_derivative,bestquality_j,bestquality_width,bestquality,steering_point, Point(img.cols/2, img.rows-img.rows/5)); delete [] angle_derivative; delete [] angles; -- cgit v1.2.1