diff options
| -rw-r--r-- | horizon_steerer.cpp | 10 | ||||
| -rw-r--r-- | mariokart.cpp | 14 |
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; |