From eee3e9ab4df6dec997b1c7bbe7b838b219725411 Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Thu, 29 Nov 2012 00:59:20 +0100 Subject: =?UTF-8?q?noch=20sch=C3=B6ner=20(immernoch=20WIP)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- detect_road_borders.cpp | 573 ++++++++++++++++++++++++++---------------------- 1 file changed, 312 insertions(+), 261 deletions(-) diff --git a/detect_road_borders.cpp b/detect_road_borders.cpp index 9b1e731..81f4ca5 100644 --- a/detect_road_borders.cpp +++ b/detect_road_borders.cpp @@ -239,10 +239,318 @@ double only_retain_largest_region(Mat img, int* size) else return (double)area_cnt[maxi2]/(double)area_cnt[maxi]; } -#define AREA_HISTORY 10 #define SMOOTHEN_BOTTOM 25 #define SMOOTHEN_MIDDLE 10 #define ANG_SMOOTH 9 +void 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. + + vector > contours; + vector hierarchy; + + findContours(img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE, Point(0, 0)); + + // Draw contours + drawing = Mat::zeros( img.size(), CV_8UC3 ); + + for( int i = 0; i< contours.size(); i++ ) + { + Scalar color; + + + if (hierarchy[i][3]<0) // no parent + color=Scalar(255,255,255); + else // this is a sub-contour which is actually irrelevant for our needs + color=Scalar(255,0,0); + + drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, Point() ); + } + + + + for (int road_contour_idx=0; road_contour_idx& contour = contours[road_contour_idx]; // just a shorthand + + if (!contour.size()>0) continue; // should never happen. + + + // find highest and lowest contour point. (where "low" means high y-coordinate) + int low_y=0, low_idx=-1; + int high_y=drawing.rows; + + for (int j=0;j low_y) + { + low_y=contour[j].y; + low_idx=j; + } + if (contour[j].y < high_y) + high_y=contour[j].y; + } + + assert(low_idx!=0); + + + + + + // make the contour go "from bottom upwards and then downwards back to bottom". + std::rotate(contour.begin(),contour.begin()+low_idx,contour.end()); + + // create contour map + for (int j=0;j= contour.size()) j1-=contour.size(); + int j2=(j-smoothen); while (j2 < 0) j2+=contour.size(); + + + // calculate angle, adjust it to be within [0, 360) + angles[j] = atan2(contour[j1].y - contour[j2].y, contour[j1].x - contour[j2].x) * 180/3.141592654; + if (angles[j]<0) angles[j]+=360; + + + // irrelevant drawing stuff + int r,g,b; + hue2rgb(angles[j], &r, &g, &b); + circle(drawing, contour[j], 2, Scalar(b,g,r)); + int x=drawing.cols-drawing.cols*(j-first_nonbottom_idx)/(contour.size()-first_nonbottom_idx); + line(drawing,Point(x,0), Point(x,10), Scalar(b,g,r)); + } + + // calculate derivative of angle for each nonbottom contour point + double* angle_derivative = new double[contour.size()]; + for (int j=first_nonbottom_idx+ANG_SMOOTH; j=360) ang_diff-=360; + if (ang_diff>=180) ang_diff=360-ang_diff; + + angle_derivative[j] = (double)ang_diff / ANG_SMOOTH; + + + + + // irrelevant drawing stuff + int x=drawing.cols-drawing.cols*(j-first_nonbottom_idx)/(contour.size()-first_nonbottom_idx); + int c=abs(20* ang_diff/ANG_SMOOTH); + Scalar col=(c<256) ? Scalar(255-c,255-c,255) : Scalar(255,0,255); + line(drawing, Point(x,12), Point(x,22), col); + + int y=25+40-2*ang_diff/ANG_SMOOTH; + line(drawing, Point(x,y), Point(x,y), Scalar(255,255,255)); + circle(drawing, contour[j], 2, col); + } + + // poorly extrapolate the ANG_SMOOTH margins + for (int j=first_nonbottom_idx; j= MAX_HYST * max_deriv \forall i \in [a,b] and + // ang_deriv[a-1,2,3], ang_deriv[b+1,2,3] < MAX_HYST * max_deriv + // where max_deriv = max_{i \in [a,b]} ang_deriv[i]; + for (int j=3; j lastmax) lastmax=angle_derivative[j]; + + if (angle_derivative[j] < MAX_HYST*lastmax && // found the end of the max. region + angle_derivative[j+1] < MAX_HYST*lastmax && + angle_derivative[j+2] < MAX_HYST*lastmax) + { + if (lastmax > 5) // threshold the maximum. + { + // search backward for the begin of that maximum region + int j0; + for (j0=j-1; j0>=0; j0--) + if (angle_derivative[j0] < MAX_HYST*lastmax && + angle_derivative[j0-1] < MAX_HYST*lastmax && + angle_derivative[j0-2] < MAX_HYST*lastmax) + break; + + // maximum region is [j0; j] + + double median_of_max_region = (double)angle_derivative[(j+j0)/2]; + + // calculate quality of that maximum. quality is high, if + // 1) the maximum has a high value AND + // 2) the corresponding point's y-coordinates are near the top image border AND + // 3) the corresponding point's x-coordinates are near the middle of the image, if in doubt + int middle_x = drawing.cols/2; + int distance_from_middle_x = abs(drawing.cols/2 - contour[j].x); + double quality = median_of_max_region + * linear( contour[j].y, high_y, 1.0, high_y+ (drawing.rows-high_y)/10, 0.0, true) // excessively punish points far away from the top border + * linear( distance_from_middle_x, 0.8*middle_x, 1.0, middle_x, 0.6, true); // moderately punish point far away from the x-middle. + + // keep track of the best point + if (quality>bestquality) + { + bestquality=quality; + bestquality_max=lastmax; + bestquality_j=(j+j0)/2; + bestquality_width=j-j0; + } + + + // irrelevant drawing stuff + int x=drawing.cols-drawing.cols*((j+j0)/2-first_nonbottom_idx)/(contour.size()-first_nonbottom_idx); + line(drawing, Point(x,25+40-3*quality), Point(x, 25+40), Scalar(0,255,0)); + circle(drawing, contour[(j+j0)/2], 1, Scalar(128,0,0)); + } + + lastmax=-999999; // reset lastmax, so the search can go on + } + } + + // now bestquality_j holds the index of the point with the best quality. + + circle(drawing, contour[bestquality_j], 3, Scalar(255,255,0)); + circle(drawing, contour[bestquality_j], 2, Scalar(255,255,0)); + circle(drawing, contour[bestquality_j], 1, Scalar(255,255,0)); + circle(drawing, contour[bestquality_j], 0, Scalar(255,255,0)); + + int antisaturation = 200-(200* bestquality/10.0); + if (antisaturation<0) antisaturation=0; + for (int j=0;j=0; xx--) + { + int intersection2 = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contour[bestquality_j].y, contour_map); + if (intersection2<0) // won't happen anyway + break; + + if (intersection2>=bestquality_j) // now we intersect the opposite (=left) border + { + if (contour[intersection2].y>=lastheight) // we intersect at a lower = worse point? + xx++; // then undo last step + + break; + } + lastheight=contour[intersection2].y; + } + } + else if (intersection > bestquality_j) // too far on the left == intersecting the left border + { + // rotate the line to the right till it gets better + for (; xx=lastheight) // we intersect at a lower = worse point? + xx--; // then undo last step + + break; + } + lastheight=contour[intersection2].y; + } + } + // else // we directly met the bestquality point, i.e. where we wanted to go to. + // do nothing + + // drawing stuff: + // now find the intrsection point of our line with the contour (just for drawing it nicely) + int steering_point = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contour[bestquality_j].y, contour_map, false); + if (steering_point>=0) // should be always true + line(drawing, contour[steering_point], Point(drawing.cols/2, drawing.rows-drawing.rows/5), Scalar(0,255,255)); + } + + cout << "bestquality_width="< low_y) - { - low_y=contour[j].y; - low_idx=j; - } - if (contour[j].y < high_y) - high_y=contour[j].y; - } - - assert(low_idx!=0); - - - - - - // make the contour go "from bottom upwards and then downwards back to bottom". - std::rotate(contour.begin(),contour.begin()+low_idx,contour.end()); - - // create contour map - for (int j=0;j= contour.size()) j1-=contour.size(); - int j2=(j-smoothen); while (j2 < 0) j2+=contour.size(); - - - // calculate angle, adjust it to be within [0, 360) - angles[j] = atan2(contour[j1].y - contour[j2].y, contour[j1].x - contour[j2].x) * 180/3.141592654; - if (angles[j]<0) angles[j]+=360; - - - // irrelevant drawing stuff - int r,g,b; - hue2rgb(angles[j], &r, &g, &b); - circle(drawing, contour[j], 2, Scalar(b,g,r)); - int x=drawing.cols-drawing.cols*(j-first_nonbottom_idx)/(contour.size()-first_nonbottom_idx); - line(drawing,Point(x,0), Point(x,10), Scalar(b,g,r)); - } - - // calculate derivative of angle for each nonbottom contour point - double* angle_derivative = new double[contour.size()]; - for (int j=first_nonbottom_idx+ANG_SMOOTH; j=360) ang_diff-=360; - if (ang_diff>=180) ang_diff=360-ang_diff; - - angle_derivative[j] = (double)ang_diff / ANG_SMOOTH; - - - - - // irrelevant drawing stuff - int x=drawing.cols-drawing.cols*(j-first_nonbottom_idx)/(contour.size()-first_nonbottom_idx); - int c=abs(20* ang_diff/ANG_SMOOTH); - Scalar col=(c<256) ? Scalar(255-c,255-c,255) : Scalar(255,0,255); - line(drawing, Point(x,12), Point(x,22), col); - - int y=25+40-2*ang_diff/ANG_SMOOTH; - line(drawing, Point(x,y), Point(x,y), Scalar(255,255,255)); - circle(drawing, contour[j], 2, col); - } - - // poorly extrapolate the ANG_SMOOTH margins - for (int j=first_nonbottom_idx; j lastmax) lastmax=angle_derivative[j]; - if (angle_derivative[j] < MAX_HYST*lastmax && angle_derivative[j+1] < MAX_HYST*lastmax && angle_derivative[j+2] < MAX_HYST*lastmax) - { - int j0=-1; - for (j0=j-1; j0>=0; j0--) - if (angle_derivative[j0] < MAX_HYST*lastmax && angle_derivative[j0-1] < MAX_HYST*lastmax && angle_derivative[j0-2] < MAX_HYST*lastmax) - break; - - if (lastmax > 5) - { - // the maximum area goes from j0 to j - int x=drawing.cols-drawing.cols*((j+j0)/2-first_nonbottom_idx)/(contour.size()-first_nonbottom_idx); - - double quality = ((double)angle_derivative[(j+j0)/2]) * linear(contour[j].y, high_y, 1.0, high_y+ (drawing.rows-high_y)/10, 0.0, true) - * linear( abs(drawing.cols/2 - contour[j].x), 0.8*drawing.cols/2, 1.0, drawing.cols/2, 0.6, true); - - if (quality>bestquality) - { - bestquality=quality; - bestquality_max=lastmax; - bestquality_j=(j+j0)/2; - bestquality_width=j-j0; - } - - line(drawing, Point(x,25+40-3*quality), Point(x, 25+40), Scalar(0,255,0)); - circle(drawing, contour[(j+j0)/2], 1, Scalar(128,0,0)); - } - lastmax=-999999; - } - } - - circle(drawing, contour[bestquality_j], 3, Scalar(255,255,0)); - circle(drawing, contour[bestquality_j], 2, Scalar(255,255,0)); - circle(drawing, contour[bestquality_j], 1, Scalar(255,255,0)); - circle(drawing, contour[bestquality_j], 0, Scalar(255,255,0)); - - int antisaturation = 200-(200* bestquality/10.0); - if (antisaturation<0) antisaturation=0; - for (int j=0;j=0) // should always be true - { - 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) // im pinken bereich, also zu weit rechts - { - for (; xx>=0; xx--) - { - int intersection2 = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contour[bestquality_j].y, contour_map); - if (intersection2<0) - break; - if (intersection2>=bestquality_j) // im gegenüberliegenden bereich? - { - if (contour[intersection2].y>=lastheight) xx++; // undo last step - break; - } - lastheight=contour[intersection2].y; - } - } - else if (intersection > bestquality_j) // im grünen bereich, also zu weit links - { - for (; xx=lastheight) xx--; // undo last step - break; - } - lastheight=contour[intersection2].y; - } - } - // else // genau den horizontpunkt getroffen - // do nothing - - int steering_point = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contour[bestquality_j].y, contour_map, false); - if (steering_point>=0) // should be always true - line(drawing, contour[steering_point], Point(drawing.cols/2, drawing.rows-drawing.rows/5), Scalar(0,255,255)); - } - - cout << "bestquality_width="<