summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--Makefile4
-rw-r--r--detect_road_borders.cpp558
2 files changed, 299 insertions, 263 deletions
diff --git a/Makefile b/Makefile
index 58da5bf..210ac2a 100644
--- a/Makefile
+++ b/Makefile
@@ -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