horizon steerer lenkt jetzt wirklich. teils schlechter, teils besser.

5 files changed, 60 insertions, 20 deletions

diff --git a/horizon_steerer.cpp b/horizon_steerer.cpp
index f6fdba1..3a41248 100644
--- a/horizon_steerer.cpp
+++ b/horizon_steerer.cpp
@@ -62,8 +62,26 @@ void HorizonSteerer::process_image(const Mat& img_)
 
 
 	Mat drawing;
-	find_steering_point(img, Point(img.cols/2, img.rows-2*img.rows/5), my_contour_map, drawing, &confidence);
-	steer_value=0.0; // TODO
+	Point origin_point(img.cols/2, img.rows-2*img.rows/10);
+	Point steering_point = find_steering_point(img, origin_point, my_contour_map, drawing, &confidence);
+	if (confidence > 0.0 && steering_point != origin_point)
+	{
+		// negative values mean "steer left", positive mean "steer right"
+		double angle = -atan2(origin_point.x - steering_point.x, origin_point.y- steering_point.y) *180/3.1415192654;
+		if (angle > 90 || angle < -90)
+		{
+			confidence=0.0;
+			steer_value=0.0;
+		}
+		else
+		{
+			steer_value=flopow(angle/90.0,1)/2  ;
+		}
+	}
+	else
+	{
+		steer_value=0.0;
+	}
 	imshow("drawing",drawing);
 }
 
@@ -627,8 +645,8 @@ void HorizonSteerer::draw_it_all(Mat drawing, vector< vector<Point> >& contours,
 #define SMOOTHEN_BOTTOM 20
 #define SMOOTHEN_MIDDLE 7
 #define ANG_SMOOTH 9
-// 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)
+// return the point to steer to, or origin_point upon error
+Point 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)
 {
@@ -653,7 +671,7 @@ int HorizonSteerer::find_steering_point(Mat orig_img, Point origin_point, int**
 	{
 		drawContours(drawing, contours, -1, Scalar(255,0,0), 1, 8, hierarchy);
 		*confidence=0.0;
-		return -1;
+		return origin_point;
 	}
 	                                                 
 	init_contour_map(contour, contour_map);
@@ -667,7 +685,7 @@ int HorizonSteerer::find_steering_point(Mat orig_img, Point origin_point, int**
 		drawContours(drawing, contours, -1, Scalar(255,0,0), 1, 8, hierarchy);
 		*confidence=0.0;
 		delete [] angles;
-		return -1;
+		return origin_point;
 	}
 	
 	int bestquality, bestquality_j, bestquality_width, bestquality_max;
@@ -678,7 +696,7 @@ int HorizonSteerer::find_steering_point(Mat orig_img, Point origin_point, int**
 		*confidence=0.0;
 		delete [] angles;
 		delete [] angle_derivative;
-		return -1;
+		return origin_point;
 	}
 	
 	// now we have a naive steering point. the way to it might lead
@@ -695,6 +713,14 @@ int HorizonSteerer::find_steering_point(Mat orig_img, Point origin_point, int**
 	delete [] angle_derivative;
 	delete [] angles;
 	
-	return steering_point;
+	if (steering_point>=0)
+	{
+		return contour[steering_point];
+	}
+	else
+	{
+		*confidence=0.0;
+		return origin_point;
+	}
 }
 
diff --git a/horizon_steerer.h b/horizon_steerer.h
index 0aa19b7..ac9d36f 100644
--- a/horizon_steerer.h
+++ b/horizon_steerer.h
@@ -56,7 +56,7 @@ private:
 	                 int steering_point, Point origin_point, double confidence);
 	void draw_angles_and_contour(Mat drawing, vector< vector<Point> >& contours, const vector<Vec4i>& hierarchy, int first_nonbottom_idx, vector<Point>& contour,
 	                             double* angles, double* angle_derivative);
-	int find_steering_point(Mat orig_img, Point origin_point, int** contour_map, Mat& drawing, double* confidence);
+	Point find_steering_point(Mat orig_img, Point origin_point, int** contour_map, Mat& drawing, double* confidence);
 
 	int xlen;
 	int ylen;
diff --git a/mariokart.cpp b/mariokart.cpp
index 4b8bee5..eb20bff 100644
--- a/mariokart.cpp
+++ b/mariokart.cpp
@@ -34,6 +34,7 @@
 
 #include "util.h"
 #include "steer_interface.h"
+#include "steer_accumulator.h"
 #include "naive_steerer.h"
 #include "horizon_steerer.h"
 #include "road_thresholder_iface.h"
@@ -306,8 +307,14 @@ joystick.reset();
 	}
 
   
-  SteerIface* steerer = new NaiveSteerer(xlen/2, 0.58*ylen);
+  SteerAccumulator* steerer = new SteerAccumulator();
+  NaiveSteerer* naive_steerer = new NaiveSteerer(xlen/2, 0.58*ylen);
   HorizonSteerer* hor_steerer = new HorizonSteerer(xlen,ylen);
+
+  steerer->add_steerer(naive_steerer, 1.0);
+  steerer->add_steerer(hor_steerer,   5.0);
+
+
   RoadThresholderIface* road_thresholder = new RoadThresholder();
   
   while(1)
diff --git a/steer_accumulator.cpp b/steer_accumulator.cpp
index 20dfc5a..ffc5909 100644
--- a/steer_accumulator.cpp
+++ b/steer_accumulator.cpp
@@ -23,22 +23,22 @@
 
 using namespace std;
 
-void SteerAccumulator::add_steerer(SteerIface* steerer)
+void SteerAccumulator::add_steerer(SteerIface* steerer, double weight)
 {
-	steerers.push_back(steerer);
+	steerers.push_back(entry(steerer,weight));
 }
 
 void SteerAccumulator::process_image(const Mat& img)
 {
-	for (list<SteerIface*>::iterator it = steerers.begin(); it!=steerers.end(); ++it)
-		(*it)->process_image(img);
+	for (list<entry>::iterator it = steerers.begin(); it!=steerers.end(); ++it)
+		it->st->process_image(img);
 }
 
 double SteerAccumulator::get_steer_data()
 {
 	double sum=0;
-	for (list<SteerIface*>::iterator it = steerers.begin(); it!=steerers.end(); ++it)
-		sum+=(*it)->get_steer_data() * (*it)->get_confidence();
+	for (list<entry>::iterator it = steerers.begin(); it!=steerers.end(); ++it)
+		sum+=it->st->get_steer_data() * it->st->get_confidence() * it->weight;
 	
 	double confidence = get_confidence();
 	
@@ -48,7 +48,7 @@ double SteerAccumulator::get_steer_data()
 double SteerAccumulator::get_confidence()
 {
 	double sum=0;
-	for (list<SteerIface*>::iterator it = steerers.begin(); it!=steerers.end(); ++it)
-		sum+=(*it)->get_confidence();
+	for (list<entry>::iterator it = steerers.begin(); it!=steerers.end(); ++it)
+		sum+=it->st->get_confidence() * it->weight;
 	return sum;
 }
diff --git a/steer_accumulator.h b/steer_accumulator.h
index 94ba082..cd0299d 100644
--- a/steer_accumulator.h
+++ b/steer_accumulator.h
@@ -34,14 +34,21 @@ class SteerAccumulator : SteerIface
 	public:
 		virtual ~SteerAccumulator() {};
 		
-		void add_steerer(SteerIface* steerer);
+		void add_steerer(SteerIface* steerer, double weight=1.0);
 		
 		virtual void process_image(const Mat& img);
 		virtual double get_steer_data();
 		virtual double get_confidence();
 	
 	private:
-		std::list<SteerIface*> steerers;
+		struct entry
+		{
+			SteerIface* st;
+			double weight;
+			
+			entry(SteerIface* s, double w) { st=s; weight=w; }
+		};
+		std::list<entry> steerers;
 };
 
 #endif