diff options
| -rw-r--r-- | Makefile | 4 | ||||
| -rw-r--r-- | detect_road_borders.cpp | 558 |
2 files changed, 299 insertions, 263 deletions
@@ -1,11 +1,11 @@ all: detect_road_borders mariokart01 detect_road_borders: detect_road_borders.cpp - g++ `pkg-config --libs --cflags opencv` $< -o $@ + g++ `pkg-config --libs --cflags opencv` -g $< -o $@ test_detect: detect_road_borders ./detect_road_borders test.mpg mariokart01: mariokart01.cpp - g++ `pkg-config --libs --cflags opencv` -lxcb -lpthread $< -o $@ + g++ `pkg-config --libs --cflags opencv` -lxcb -lpthread -g $< -o $@ diff --git a/detect_road_borders.cpp b/detect_road_borders.cpp index b8b1dba..4ee2fc8 100644 --- a/detect_road_borders.cpp +++ b/detect_road_borders.cpp @@ -7,6 +7,10 @@ using namespace std; using namespace cv; +void set_pixel(Mat m, Point p, Scalar color) +{ + line(m,p,p,color); +} int find_intersection_index(int x0, int y0, int x1, int y1, int** contour_map, bool stop_at_endpoint=true) // bresenham aus der dt. wikipedia // returns: the point's index where the intersection happened, or a negative number if no intersection. @@ -238,308 +242,340 @@ double only_retain_largest_region(Mat img, int* size) else return (double)area_cnt[maxi2]/(double)area_cnt[maxi]; } -#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 + +vector<Point>& prepare_and_get_contour(int xlen, int ylen, vector< vector<Point> >& contours, const vector<Vec4i>& hierarchy, + int* low_y, int* low_idx, int* high_y, int* first_nonbottom_idx) { - Mat img; - orig_img.copyTo(img); // this is needed because findContours destroys its input. + assert(low_y!=NULL); + assert(low_idx!=NULL); + assert(high_y!=NULL); + assert(first_nonbottom_idx!=NULL); - vector<vector<Point> > contours; - vector<Vec4i> hierarchy; + + // find index of our road contour + int road_contour_idx=-1; + for (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. + break; - findContours(img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE, Point(0, 0)); - // Draw contours - drawing = Mat::zeros( img.size(), CV_8UC3 ); + assert(road_contour_idx>=0 && road_contour_idx<contours.size()); + assert(contours[road_contour_idx].size()>0); + vector<Point>& contour = contours[road_contour_idx]; // just a shorthand + + // our road is now in contour. - for( int i = 0; i< contours.size(); i++ ) + + // find highest and lowest contour point. (where "low" means high y-coordinate) + *low_y=0; *low_idx=0; + *high_y=ylen; + + for (int j=0;j<contour.size(); j++) { - Scalar color; + 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; + } + } + + // make the contour go "from bottom upwards and then downwards back to bottom". + std::rotate(contour.begin(),contour.begin()+*low_idx,contour.end()); - 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); + *first_nonbottom_idx = 0; + for (;*first_nonbottom_idx<contour.size();*first_nonbottom_idx++) + if (contour[*first_nonbottom_idx].y < contour[0].y-1) break; - drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, Point() ); - } + // indices 0 to *first_nonbottom_idx-1 is now the bottom line of our contour. + + return contour; +} +void init_contour_map(const vector<Point>& contour, int** contour_map, int xlen, int ylen) +{ + for (int j=0;j<xlen;j++) // zero it + memset(contour_map[j],0,ylen*sizeof(**contour_map)); + + for (int j=0;j<contour.size(); j++) // fill it + contour_map[contour[j].x][contour[j].y]=j; +} +// returns a new double[] +double* calc_contour_angles(const vector<Point>& contour, int first_nonbottom_idx, int ylen, int smoothen_middle, int smoothen_bottom) +{ + // 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, ylen/2 ,smoothen_middle, ylen,smoothen_bottom, true); + - 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); - - - - + // 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(); - // 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; + // 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; + } + return angles; +} - - line(drawing, Point(0,high_y), Point(drawing.cols,high_y), Scalar(127,127,127)); - - - +double* calc_angle_deriv(double* angles, int first_nonbottom_idx, int size, int ang_smooth) +{ + // calculate derivative of angle for each nonbottom contour point + double* angle_derivative = new double[size]; + for (int j=first_nonbottom_idx+ang_smooth; j<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; - 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; + angle_derivative[j] = (double)ang_diff / ang_smooth; + } + + // 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=size-ang_smooth; j<size; j++) angle_derivative[j]=angle_derivative[size-ang_smooth-1]; + + return angle_derivative; +} + + + +int find_bestquality_index(const vector<Point>& contour, double* angle_derivative, int high_y, int first_nonbottom_idx, Mat& drawing, + int* bestquality_j_out, int* bestquality_width_out, int* bestquality_out) +{ + assert(bestquality_out!=NULL); + assert(bestquality_j_out!=NULL); + assert(bestquality_width_out!=NULL); + + 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]; - - // calculate directional angle for each nonbottom contour point - double* angles = new double[contour.size()]; - for (int j=first_nonbottom_idx; j<contour.size(); 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. { - 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; + // 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]; - // 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; + // 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-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); + 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)); } - // 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++) + lastmax=-999999; // reset lastmax, so the search can go on + } + } + // now bestquality_j holds the index of the point with the best quality. + + *bestquality_out = bestquality; + *bestquality_j_out = bestquality_j; + *bestquality_width_out = bestquality_width; +} + +int find_ideal_line(int xlen, int ylen, vector<Point>& contour, int** contour_map, int bestquality_j, Mat drawing) +// 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, + contour[bestquality_j].x, contour[bestquality_j].y, contour_map); + int steering_point=-1; + + if (intersection<0) + { + cout << "THIS SHOULD NEVER HAPPEN" << endl; + return -1; + } + 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--) { - // search forward for a maximum, and the end of a maximum region. - if (angle_derivative[j] > lastmax) lastmax=angle_derivative[j]; + 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 (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 (intersection2>=bestquality_j) // now we intersect the opposite (=left) border { - 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]; + if (contour[intersection2].y>=lastheight) // we intersect at a lower = worse point? + xx++; // then undo last step - // 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 + break; } + lastheight=contour[intersection2].y; } - - // 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 + } + 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++) { - 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 + 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 { - // 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 (contour[intersection2].y>=lastheight) // we intersect at a lower = worse point? + xx--; // then undo last step - 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; - } + break; } - // 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)); + lastheight=contour[intersection2].y; } - - cout << "bestquality_width="<<bestquality_width <<",\tquality="<<bestquality<<",\t"<<"raw max="<<bestquality_max - <<endl<<endl<<endl<<endl; - - - delete [] angle_derivative; - delete [] angles; } + // else // we directly met the bestquality point, i.e. where we wanted to go to. + // do nothing + + return find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contour[bestquality_j].y, contour_map, false); + } +} + - /*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++) +#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. + + vector<vector<Point> > contours; + vector<Vec4i> 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<Point>& 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 + for (int j=first_nonbottom_idx; j<contour.size(); j++) { - double s=sin((double)a*3.141592654/180.0); - double c=cos((double)a*3.141592654/180.0); + int x=drawing.cols-drawing.cols*(j-first_nonbottom_idx)/(contour.size()-first_nonbottom_idx); + + // draw angle as color bar 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) ); - }*/ + hue2rgb(angles[j], &r, &g, &b); + line(drawing,Point(x,0), Point(x,10), Scalar(b,g,r)); + + // draw derivation of angle as color bar + int c=abs(20* angle_derivative[j]); + Scalar col=(c<256) ? Scalar(255-c,255-c,255) : Scalar(255,0,255); + line(drawing, Point(x,12), Point(x,22), col); + + // and as x-y-graph + int y=25+40-2*angle_derivative[j]; + set_pixel(drawing, Point(x,y), Scalar(255,255,255)); + + // draw into contour + //circle(drawing, contour[j], 2, col); + set_pixel(drawing, contour[j], col); + } + + int bestquality, bestquality_j, bestquality_width; + find_bestquality_index(contour, angle_derivative, 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. + + + 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++) + set_pixel(drawing, contour[j], Scalar(255,antisaturation,255)); + for (int j=bestquality_j+bestquality_width/2;j<contour.size();j++) + set_pixel(drawing, contour[j], Scalar(antisaturation,255,antisaturation)); + + line(drawing, contour[bestquality_j], Point(img.cols/2, img.rows-img.rows/5), Scalar(0,64,64)); + + int steering_point=find_ideal_line(img.cols,img.rows, contour, contour_map, bestquality_j, drawing); + + 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)); + + + delete [] angle_derivative; + delete [] angles; + return steering_point; } #define AREA_HISTORY 10 |