diff options
| -rw-r--r-- | detect_road_borders.cpp | 573 |
1 files 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<vector<Point> > contours; + vector<Vec4i> 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<contours.size(); road_contour_idx++ ) + if (hierarchy[road_contour_idx][3]<0) // this will be true for exactly one road_contour_idx. + { + vector<Point>& 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<contour.size(); j++) + { + if (contour[j].y > 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<img.cols;j++) // zero it + memset(contour_map[j],0,img.rows*sizeof(**contour_map)); + for (int j=0;j<contour.size(); j++) // fill it + contour_map[contour[j].x][contour[j].y]=j; + + /*int j; + for (j=0;j<contour.size();j++) + if (contour[j].y < contour[0].y-1) break; + for (;j<contour.size();j++) + circle(drawing, contour[j], 2, Scalar( 0,255-( j *255/contour.size()),( j *255/contour.size()))); + */ + + line(drawing, Point(0,high_y), Point(drawing.cols,high_y), Scalar(127,127,127)); + + + + + int first_nonbottom_idx = 0; + for (;first_nonbottom_idx<contour.size();first_nonbottom_idx++) + if (contour[first_nonbottom_idx].y < contour[0].y-1) break; + + + // calculate directional angle for each nonbottom contour point + double* angles = new double[contour.size()]; + for (int j=first_nonbottom_idx; j<contour.size(); j++) + { + int smoothen=linear(contour[j].y, img.rows/2 ,SMOOTHEN_MIDDLE, img.rows,SMOOTHEN_BOTTOM, true); + + + // calculate left and right point for the difference quotient, possibly wrap. + int j1=(j+smoothen); while (j1 >= 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<contour.size()-ANG_SMOOTH; j++) + { + // calculate angular difference, adjust to be within [0;360) and take the shorter way. + double ang_diff = angles[j+ANG_SMOOTH]-angles[j-ANG_SMOOTH]; + while (ang_diff<0) ang_diff+=360; + while (ang_diff>=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<first_nonbottom_idx+ANG_SMOOTH; j++) angle_derivative[j]=angle_derivative[first_nonbottom_idx+ANG_SMOOTH]; + for (int j=contour.size()-ANG_SMOOTH; j<contour.size(); j++) angle_derivative[j]=angle_derivative[contour.size()-ANG_SMOOTH-1]; + + + + + + double lastmax=-999999; + double bestquality=0.0; + double bestquality_max=0.0; + int bestquality_j=-1; + int bestquality_width=-1; + + #define MAX_HYST 0.8 + // search for "maximum regions"; i.e. intervals [a,b] with + // ang_deriv[i] >= 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<contour.size()-3; j++) + { + // search forward for a maximum, and the end of a maximum region. + if (angle_derivative[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<bestquality_j-bestquality_width/2;j++) + circle(drawing, contour[j], 2, Scalar(255,antisaturation,255)); + for (int j=bestquality_j+bestquality_width/2;j<contour.size();j++) + circle(drawing, contour[j], 2, Scalar(antisaturation,255,antisaturation)); + + line(drawing, contour[bestquality_j], Point(drawing.cols/2, drawing.rows-drawing.rows/5), Scalar(0,64,64)); + + + + + // TODO: the below code is crappy, slow, and uses brute force. did i mention it's crappy and slow? + + int intersection = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, + contour[bestquality_j].x, contour[bestquality_j].y, contour_map); + if (intersection<0) + { + cout << "THIS SHOULD NEVER HAPPEN" << endl; + } + 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 + { + // rotate the line to the left till it gets better + 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) // 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<drawing.cols; 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 (=right) border + { + if (contour[intersection2].y>=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="<<bestquality_width <<",\tquality="<<bestquality<<",\t"<<"raw max="<<bestquality_max + <<endl<<endl<<endl<<endl; + + + delete [] angle_derivative; + delete [] angles; + } + + /*Point midpoint=Point(drawing.cols/2, 250); // color circle, uncomment if you want to understand the colored bar on the top border. + for (int a=0; a<360; a++) + { + double s=sin((double)a*3.141592654/180.0); + double c=cos((double)a*3.141592654/180.0); + int r,g,b; + hue2rgb(a, &r, &g, &b); + line(drawing,midpoint-Point(c*5, s*5), midpoint-Point(c*30, s*30),Scalar(b,g,r) ); + }*/ +} + +#define AREA_HISTORY 10 + int alertcnt=21; int main(int argc, char* argv[]) { @@ -303,265 +611,8 @@ int main(int argc, char* argv[]) erode(tmp, thres, erode_kernel); - - Mat thres_tmp; - thres.copyTo(thres_tmp); // this is needed because findContours destroys its input. - - vector<vector<Point> > contours; - vector<Vec4i> hierarchy; - - findContours(thres_tmp, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE, Point(0, 0)); - - // Draw contours - Mat drawing = Mat::zeros( thres_tmp.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<contours.size(); road_contour_idx++ ) - if (hierarchy[road_contour_idx][3]<0) // this will be true for exactly one road_contour_idx. - { - vector<Point>& 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<contour.size(); j++) - { - if (contour[j].y > 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<frame.cols;j++) // zero it - memset(contour_map[j],0,frame.rows*sizeof(**contour_map)); - for (int j=0;j<contour.size(); j++) // fill it - contour_map[contour[j].x][contour[j].y]=j; - - /*int j; - for (j=0;j<contour.size();j++) - if (contour[j].y < contour[0].y-1) break; - for (;j<contour.size();j++) - circle(drawing, contour[j], 2, Scalar( 0,255-( j *255/contour.size()),( j *255/contour.size()))); - */ - - line(drawing, Point(0,high_y), Point(drawing.cols,high_y), Scalar(127,127,127)); - - - - - int first_nonbottom_idx = 0; - for (;first_nonbottom_idx<contour.size();first_nonbottom_idx++) - if (contour[first_nonbottom_idx].y < contour[0].y-1) break; - - - // calculate directional angle for each nonbottom contour point - double* angles = new double[contour.size()]; - for (int j=first_nonbottom_idx; j<contour.size(); j++) - { - int smoothen=linear(contour[j].y, thres.rows/2 ,SMOOTHEN_MIDDLE, thres.rows,SMOOTHEN_BOTTOM, true); - - - // calculate left and right point for the difference quotient, possibly wrap. - int j1=(j+smoothen); while (j1 >= 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<contour.size()-ANG_SMOOTH; j++) - { - // calculate angular difference, adjust to be within [0;360) and take the shorter way. - double ang_diff = angles[j+ANG_SMOOTH]-angles[j-ANG_SMOOTH]; - while (ang_diff<0) ang_diff+=360; - while (ang_diff>=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<first_nonbottom_idx+ANG_SMOOTH; j++) angle_derivative[j]=angle_derivative[first_nonbottom_idx+ANG_SMOOTH]; - for (int j=contour.size()-ANG_SMOOTH; j<contour.size(); j++) angle_derivative[j]=angle_derivative[contour.size()-ANG_SMOOTH-1]; - - - - - - double lastmax=-999999; - double bestquality=0.0; - double bestquality_max=0.0; - int bestquality_j=-1; - int bestquality_width=-1; - - #define MAX_HYST 0.8 - for (int j=3;j<contour.size()-3;j++) - { - if (angle_derivative[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<bestquality_j-bestquality_width/2;j++) - circle(drawing, contour[j], 2, Scalar(255,antisaturation,255)); - for (int j=bestquality_j+bestquality_width/2;j<contour.size();j++) - circle(drawing, contour[j], 2, Scalar(antisaturation,255,antisaturation)); - - line(drawing, contour[bestquality_j], Point(drawing.cols/2, drawing.rows-drawing.rows/5), Scalar(0,64,64)); - - int intersection = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, contour[bestquality_j].x, contour[bestquality_j].y, contour_map); - if (intersection>=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<drawing.cols; 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 // 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="<<bestquality_width <<",\tquality="<<bestquality<<",\t"<<"raw max="<<bestquality_max - <<endl<<endl<<endl<<endl; - - - delete [] angle_derivative; - delete [] angles; - } - - /*Point midpoint=Point(drawing.cols/2, 250); // farbkreis - for (int a=0; a<360; a++) - { - double s=sin((double)a*3.141592654/180.0); - double c=cos((double)a*3.141592654/180.0); - int r,g,b; - hue2rgb(a, &r, &g, &b); - line(drawing,midpoint-Point(c*5, s*5), midpoint-Point(c*30, s*30),Scalar(b,g,r) ); - }*/ - + Mat drawing; + find_steering_point(thres, contour_map, drawing); @@ -592,7 +643,7 @@ int main(int argc, char* argv[]) cout << "frame #"<<frameno<<endl; imshow("input",thres); - imshow("contours",drawing); + imshow("drawing",drawing); // waitKey(100); waitKey(); |