diff options
| -rw-r--r-- | horizon_steerer.cpp | 42 | ||||
| -rw-r--r-- | horizon_steerer.h | 2 | ||||
| -rw-r--r-- | mariokart.cpp | 9 | ||||
| -rw-r--r-- | steer_accumulator.cpp | 16 | ||||
| -rw-r--r-- | steer_accumulator.h | 11 |
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 |