From 87a62c067dfa50f62a58503cdbec7be30241f032 Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Wed, 5 Dec 2012 00:10:29 +0100 Subject: haufenweise bugfixes, laeuft richtig rund jetzt. confidence wird angezeigt --- horizon_steerer.cpp | 144 +++++++++++++++++++++++++++++++++++++--------------- horizon_steerer.h | 7 ++- 2 files changed, 109 insertions(+), 42 deletions(-) diff --git a/horizon_steerer.cpp b/horizon_steerer.cpp index 7627c10..f6fdba1 100644 --- a/horizon_steerer.cpp +++ b/horizon_steerer.cpp @@ -49,6 +49,12 @@ void HorizonSteerer::process_image(const Mat& img_) int area_abs; double area_ratio = only_retain_largest_region(img, &area_abs); + if (area_ratio<0) // no road detected at all? + { + steer_value=0.0; + confidence=0.0; + return; + } Mat tmp; dilate(img, tmp, erode_kernel); @@ -56,13 +62,13 @@ void HorizonSteerer::process_image(const Mat& img_) Mat drawing; - double confidence; find_steering_point(img, Point(img.cols/2, img.rows-2*img.rows/5), my_contour_map, drawing, &confidence); + steer_value=0.0; // TODO imshow("drawing",drawing); } -double HorizonSteerer::get_steer_data() { return 0.0; } -double HorizonSteerer::get_confidence() { return 1.0; } +double HorizonSteerer::get_steer_data() { return steer_value; } +double HorizonSteerer::get_confidence() { return confidence; } @@ -81,17 +87,15 @@ int HorizonSteerer::find_intersection_index(int x0, int y0, int x1, int y1, int* for(;;) { - if (x0<0 || x0>=xlen || y0<0 || y0>=ylen) break; + if (x0<0 || x0>=xlen || y0<0 || y0>=ylen) return -1; - //setPixel(x0,y0); if (contour_map[x0][y0]>0) return contour_map[x0][y0]; // found intersection? if (y0+10) return contour_map[x0][y0+1]; if (x0+10) return contour_map[x0+1][y0]; - - - - if (stop_at_endpoint && x0==x1 && y0==y1) break; + if (stop_at_endpoint && x0==x1 && y0==y1) return -1; + + e2 = 2*err; if (e2 > dy) { err += dy; x0 += sx; } /* e_xy+e_x > 0 */ if (e2 < dx) { err += dx; y0 += sy; } /* e_xy+e_y < 0 */ @@ -220,12 +224,14 @@ double HorizonSteerer::only_retain_largest_region(Mat img, int* size) // in *size, if non-NULL, the size of the largest area is stored. // returns: ratio between the second-largest and largest region // 0.0 means "that's the only region", 1.0 means "both had the same size!" +// negative values mean "no area at all!" // can be interpreted as 1.0 - "confidence". { int n_regions = annotate_regions(img); + if (n_regions == 0) return -1; // calculate the area of each region - int* area_cnt = new int[n_regions]; + int* area_cnt = new int[n_regions]; // TODO: hier könnt man optimieren, wenn area_cnt==1 for (int i=0;imaxa) + if (area_cnt[i]>=maxa) { maxa=area_cnt[i]; maxi2=maxi; @@ -291,7 +297,7 @@ vector& HorizonSteerer::prepare_and_get_contour(vector< vector >& assert(low_idx!=NULL); assert(high_y!=NULL); assert(first_nonbottom_idx!=NULL); - + assert(contours.size()>=1); // find index of our road contour int road_contour_idx=-1; @@ -329,8 +335,14 @@ vector& HorizonSteerer::prepare_and_get_contour(vector< vector >& std::rotate(contour.begin(),contour.begin()+*low_idx,contour.end()); *first_nonbottom_idx = 0; - for (;*first_nonbottom_idx=0); + if (!(*first_nonbottom_idx& HorizonSteerer::prepare_and_get_contour(vector< vector >& void HorizonSteerer::init_contour_map(const vector& contour, int** contour_map) { + assert(contour_map!=NULL); + for (int j=0;j& contour, int fi return angles; } +// returns a new double[] or NULL, if contour is too small double* HorizonSteerer::calc_angle_deriv(double* angles, int first_nonbottom_idx, int size, int ang_smooth) { + if (first_nonbottom_idx+ang_smooth >= size-ang_smooth) return NULL; // not enough data for deriving! + // calculate derivative of angle for each nonbottom contour point double* angle_derivative = new double[size]; for (int j=first_nonbottom_idx+ang_smooth; j& contour, double* angle_derivative, int high_y, int first_nonbottom_idx, Mat& drawing, - int* bestquality_j_out, int* bestquality_width_out, int* bestquality_out, int* bestquality_max_out) + int* bestquality_j_out, int* bestquality_width_out, int* bestquality_out, int* bestquality_max_out) { assert(bestquality_out!=NULL); assert(bestquality_j_out!=NULL); assert(bestquality_width_out!=NULL); - double lastmax=-999999; + double lastmax=-999999; // TODO that sucks :/ double bestquality=0.0; double bestquality_max=0.0; int bestquality_j=-1; - int bestquality_width=-1; + int bestquality_width=0; #define MAX_HYST 0.8 // search for "maximum regions"; i.e. intervals [a,b] with @@ -411,7 +429,8 @@ int HorizonSteerer::find_bestquality_index(const vector& contour, double* // 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]; - // TODO BUG: better assert contour.size()>3 somewhere + + // if contour.size() is too small, the for loop is never executed, and bestquality_j stays -1. for (int j=3; j<(int)contour.size()-3; j++) { // search forward for a maximum, and the end of a maximum region. @@ -461,27 +480,33 @@ int HorizonSteerer::find_bestquality_index(const vector& contour, double* circle(drawing, contour[(j+j0)/2], 1, Scalar(128,0,0)); } - lastmax=-999999; // reset lastmax, so the search can go on + lastmax=-999999; // reset lastmax, so the search can go on. TODO: ugly. } } - // now bestquality_j holds the index of the point with the best quality. + // now bestquality_j holds the index of the point with the best quality or -1 upon failure *bestquality_out = bestquality; *bestquality_max_out = bestquality_max; *bestquality_j_out = bestquality_j; *bestquality_width_out = bestquality_width; + return bestquality_j; } +// returns index of ideal steering point or -1 on failure int HorizonSteerer::find_ideal_line(vector& contour, Point origin_point, int** contour_map, int bestquality_j) // TODO: this code is crappy, slow, and uses brute force. did i mention it's crappy and slow? { + assert(bestquality_j>=0 && bestquality_j& contour, Point origin_point, // rotate the line to the left till it gets better for (; xx>=0; xx--) { - int intersection2 = find_intersection_index(origin_point.x, origin_point.y, xx, contour[bestquality_j].y, contour_map); + int intersection2 = find_intersection_index(origin_point.x, origin_point.y, xx, contour[bestquality_j].y, contour_map, false); if (intersection2<0) // won't happen anyway + { + cout << "SHOULD NOT HAPPEN: no intersection" << endl; break; - - if (intersection2>=bestquality_j) // now we intersect the opposite (=left) border + } + else 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 + xx++; // then undo the last step break; } @@ -512,14 +539,16 @@ int HorizonSteerer::find_ideal_line(vector& contour, Point origin_point, // 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 + xx--; // then undo the last step break; } @@ -534,9 +563,8 @@ int HorizonSteerer::find_ideal_line(vector& contour, Point origin_point, } -void HorizonSteerer::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) +void HorizonSteerer::draw_angles_and_contour(Mat drawing, vector< vector >& contours, const vector& hierarchy, int first_nonbottom_idx, vector& contour, + double* angles, double* angle_derivative) { // Draw contours drawContours(drawing, contours, -1, Scalar(255,0,0), 1, 8, hierarchy); @@ -564,7 +592,13 @@ void HorizonSteerer::draw_it_all(Mat drawing, vector< vector >& contours, //circle(drawing, contour[j], 2, col); set_pixel(drawing, contour[j], col); } - +} +void HorizonSteerer::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, double confidence) +{ + draw_angles_and_contour(drawing, contours, hierarchy, first_nonbottom_idx, contour, angles, angle_derivative); + // draw the point where the left touches the right road border circle(drawing, contour[bestquality_j], 3, Scalar(255,255,0)); circle(drawing, contour[bestquality_j], 2, Scalar(255,255,0)); @@ -585,16 +619,21 @@ void HorizonSteerer::draw_it_all(Mat drawing, vector< vector >& contours, if (steering_point>=0) // should be always true line(drawing, contour[steering_point], origin_point, Scalar(0,255,255)); + + rectangle(drawing, Point(0.25*xlen-2, 100-2), Point (0.75*xlen+2, 150+2), Scalar(255,255,255)); + rectangle(drawing, Point(0.25*xlen, 100), Point ((0.25+0.5*confidence)*xlen, 150), Scalar(0,0,200*confidence+50), CV_FILLED); } #define SMOOTHEN_BOTTOM 20 #define SMOOTHEN_MIDDLE 7 #define ANG_SMOOTH 9 -// return the index of the point to steer to. -int HorizonSteerer::find_steering_point(Mat orig_img, Point origin_point, int** contour_map, Mat& drawing, double* confidence) // orig_img is a binary image +// return the index of the point to steer to, or -1 upon error +int HorizonSteerer::find_steering_point(Mat orig_img, Point origin_point, int** contour_map, Mat& drawing, double* confidence) +// orig_img is a binary image with only one region // confidence is between 0.0 (not sure at all) and 1.0 (definitely sure) { assert(confidence!=NULL); + assert(contour_map!=NULL); Mat img; orig_img.copyTo(img); // this is needed because findContours destroys its input. @@ -605,9 +644,17 @@ int HorizonSteerer::find_steering_point(Mat orig_img, Point origin_point, int** findContours(img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE, Point(0, 0)); + assert(contours.size()>=1); + int low_y, low_idx, high_y, first_nonbottom_idx; vector& contour = prepare_and_get_contour(contours, hierarchy, &low_y, &low_idx, &high_y, &first_nonbottom_idx); + if (! (first_nonbottom_idx find_bestquality_index(contour, angle_derivative, high_y, first_nonbottom_idx, drawing, + &bestquality_j, &bestquality_width, &bestquality, &bestquality_max)) + { + draw_angles_and_contour(drawing, contours, hierarchy, first_nonbottom_idx, contour, angles, angle_derivative); + *confidence=0.0; + delete [] angles; + delete [] angle_derivative; + return -1; + } // now we have a naive steering point. the way to it might lead // us offroad, however. int steering_point=find_ideal_line(contour, origin_point, contour_map, bestquality_j); + *confidence = (bestquality-1.0) / 7.0; + if (*confidence<0.0) *confidence=0; + if (*confidence>1.0) *confidence=1.0; - draw_it_all(drawing, contours, hierarchy, first_nonbottom_idx, contour, angles, angle_derivative,bestquality_j,bestquality_width,bestquality_max,steering_point, origin_point); + draw_it_all(drawing, contours, hierarchy, first_nonbottom_idx, contour, angles, angle_derivative,bestquality_j,bestquality_width,bestquality_max,steering_point, origin_point, *confidence); cout << bestquality << "\t" << bestquality_max<1.0) *confidence=1.0; return steering_point; } diff --git a/horizon_steerer.h b/horizon_steerer.h index 03e302c..0aa19b7 100644 --- a/horizon_steerer.h +++ b/horizon_steerer.h @@ -53,13 +53,18 @@ private: int* bestquality_j_out, int* bestquality_width_out, int* bestquality_out, int* bestquality_max_out); 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); + int steering_point, Point origin_point, double confidence); + void draw_angles_and_contour(Mat drawing, vector< vector >& contours, const vector& hierarchy, int first_nonbottom_idx, vector& contour, + double* angles, double* angle_derivative); int find_steering_point(Mat orig_img, Point origin_point, int** contour_map, Mat& drawing, double* confidence); int xlen; int ylen; Mat erode_kernel; int** my_contour_map; + + double confidence; + double steer_value; }; #endif -- cgit v1.2.3