From b48e848140b353752e24583d299ed1858a533f21 Mon Sep 17 00:00:00 2001
From: Florian Jung <florian.a.jung@web.de>
Date: Sat, 1 Dec 2012 10:58:25 +0100
Subject: =?UTF-8?q?noch=20sch=C3=B6ner?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Makefile                |   4 +-
 detect_road_borders.cpp | 558 ++++++++++++++++++++++++++----------------------
 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
-- 
cgit v1.2.1