horizonsteerer segfaultet nicht mehr

2 files changed, 15 insertions, 9 deletions

diff --git a/horizon_steerer.cpp b/horizon_steerer.cpp
index f15be84..7627c10 100644
--- a/horizon_steerer.cpp
+++ b/horizon_steerer.cpp
@@ -81,10 +81,12 @@ 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;
+    
     //setPixel(x0,y0);
     if (contour_map[x0][y0]>0) return contour_map[x0][y0]; // found intersection?
-    if (contour_map[x0][y0+1]>0) return contour_map[x0][y0+1];
-    if (contour_map[x0+1][y0]>0) return contour_map[x0+1][y0];
+    if (y0+1<ylen && contour_map[x0][y0+1]>0) return contour_map[x0][y0+1];
+    if (x0+1<xlen && contour_map[x0+1][y0]>0) return contour_map[x0+1][y0];
     
 		
     
@@ -408,7 +410,9 @@ int HorizonSteerer::find_bestquality_index(const vector<Point>& contour, double*
 	// 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++)
+	
+	// TODO BUG: better assert contour.size()>3 somewhere
+	for (int j=3; j<(int)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];
diff --git a/mariokart.cpp b/mariokart.cpp
index cdc3d60..4b8bee5 100644
--- a/mariokart.cpp
+++ b/mariokart.cpp
@@ -307,7 +307,7 @@ joystick.reset();
 
   
   SteerIface* steerer = new NaiveSteerer(xlen/2, 0.58*ylen);
-//  HorizonSteerer* hor_steerer = new HorizonSteerer(xlen,ylen);
+  HorizonSteerer* hor_steerer = new HorizonSteerer(xlen,ylen);
   RoadThresholderIface* road_thresholder = new RoadThresholder();
   
   while(1)
@@ -389,15 +389,17 @@ joystick.reset();
   */
 
   
-  img.row(crosshair_y)=Scalar(255,0,0);
-  img.col(crosshair_x)=Scalar(255,0,0);
-  img_thres2.row(crosshair_y)=128;
-  img_thres2.col(crosshair_x)=128;
 
 	steerer->process_image(img_thres2);
 	double steer_value = steerer->get_steer_data();
 	
-//	hor_steerer->process_image(img_thres2);
+	hor_steerer->process_image(img_thres2);
+
+
+  img.row(crosshair_y)=Scalar(255,0,0);
+  img.col(crosshair_x)=Scalar(255,0,0);
+  img_thres2.row(crosshair_y)=128;
+  img_thres2.col(crosshair_x)=128;
 
   Mat steer=Mat::zeros(20,1920,CV_8U);
   steer.col( steer.cols /2 )=128;