From 3e2c38c7a04923ce090f184cbe0d217a9e72cd6e Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Mon, 5 Nov 2012 21:59:23 +0100 Subject: detect_road_borders geaddet --- detect_road_borders.cpp | 394 ++++++++++++++++++++++++++++++++++++++++++++++++ mariokart01.cpp | 128 +++++++++++++++- track_street.cpp | 172 +++++++++++++++++++++ 3 files changed, 690 insertions(+), 4 deletions(-) create mode 100644 detect_road_borders.cpp create mode 100644 track_street.cpp diff --git a/detect_road_borders.cpp b/detect_road_borders.cpp new file mode 100644 index 0000000..2e2ad07 --- /dev/null +++ b/detect_road_borders.cpp @@ -0,0 +1,394 @@ +#include +#include +#include +#include +#include + +using namespace std; +using namespace cv; + +Mat circle_mat(int radius) +{ + Mat result(radius*2+1, radius*2+1, CV_8U); + for (int x=0; x<=result.cols/2; x++) + for (int y=0; y<=result.rows/2; y++) + { + unsigned char& p1 = result.at(result.cols/2 + x, result.rows/2 + y); + unsigned char& p2 = result.at(result.cols/2 - x, result.rows/2 + y); + unsigned char& p3 = result.at(result.cols/2 + x, result.rows/2 - y); + unsigned char& p4 = result.at(result.cols/2 - x, result.rows/2 - y); + + if ( x*x + y*y < radius*radius ) + p1=p2=p3=p4=255; + else + p1=p2=p3=p4=0; + } + + return result; +} + +void hue2rgb(float hue, int* r, int* g, int* b) +{ + double ff; + int i; + + + if (hue >= 360.0) hue = 0.0; + hue /= 60.0; + i = (int)hue; + ff = hue - i; + int x=ff*255; + + switch(i) { + case 0: + *r = 255; + *g = x; + *b = 0; + break; + case 1: + *r = 255-x; + *g = 255; + *b = 0; + break; + case 2: + *r = 0; + *g = 255; + *b = x; + break; + + case 3: + *r = 0; + *g = 255-x; + *b = 255; + break; + case 4: + *r = x; + *g = 0; + *b = 255; + break; + case 5: + default: + *r = 255; + *g = 0; + *b = 255-x; + break; + } +} + +double linear(double x, double x1, double y1, double x2, double y2, bool clip=false, double clipmin=INFINITY, double clipmax=INFINITY) +{ + if (clipmin==INFINITY) clipmin=y1; + if (clipmax==INFINITY) clipmax=y2; + if (clipmin>clipmax) { int tmp=clipmin; clipmin=clipmax; clipmax=tmp; } + + double result = (y2-y1)*(x-x1)/(x2-x1)+y1; + + if (clip) + { + if (result>clipmax) return clipmax; + else if (result> frame; + + + + + + Mat thres(frame.rows, frame.cols, CV_8UC1); + Mat tmp(frame.rows, frame.cols, CV_8UC1); + + + int area_history[AREA_HISTORY]; + for (int i=0;i> frame; + cvtColor(frame, tmp, CV_RGB2GRAY); + threshold(tmp, thres, 132, 255, THRESH_BINARY); + dilate(thres,tmp,Mat()); + erode(tmp,thres,Mat()); + erode(thres,tmp,Mat()); + dilate(tmp,thres,Mat()); + + int area_index=1; // "0" means "no area" + for (int row = 0; row(row); + + for (int col=0; col(row); + + for (int col=0; col(row); + + for (int col=0; col200) tmp=255; + *data=tmp; + } + + data++; + } + } + */ + + + // finde die größte und zweitgrößte fläche + int maxi=0, maxa=area_cnt[0], maxi2=-1; + for (int i=1;imaxa) + { + maxa=area_cnt[i]; + maxi2=maxi; + maxi=i; + } + } + + + // lösche alle bis auf die größte fläche + for (int row = 0; row(row); + + for (int col=0; col > contours; + vector hierarchy; + + findContours(thres_tmp, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE, Point(0, 0)); + /// Draw contours + Mat drawing = Mat::zeros( thres_tmp.size(), CV_8UC3 ); + + //drawContours( drawing, contours, -1, Scalar(250,0,0) , 2,8, hierarchy); + + for( int i = 0; i< contours.size(); i++ ) + { + //if (hierarchy[i][3]<0) // no parent + Scalar color = Scalar( 255 ,(hierarchy[i][3]<0)?255:0, (hierarchy[i][3]<0)?255:0 ); + drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, Point() ); + } + + for( int i = 0; i< contours.size(); i++ ) + { + if (hierarchy[i][3]<0) + { + int lowy=0, lowj=-1; + int highy=drawing.rows; + + for (int j=0;j lowy) + { + lowy=contours[i][j].y; + lowj=j; + } + if (contours[i][j].y < highy) + highy=contours[i][j].y; + } + + if (lowj!=-1) + { + std::rotate(contours[i].begin(),contours[i].begin()+lowj,contours[i].end()); + + int j; + for (j=0;j= contours[i].size()) j1-=contours[i].size(); + int j2=(j-smoothen); if (j2 < 0) j2+=contours[i].size(); + + + angles[j] = atan2(contours[i][j1].y - contours[i][j2].y, contours[i][j1].x - contours[i][j2].x) * 180 /3.141592654; + if (angles[j]<0) angles[j]+=360; + int r,g,b; + hue2rgb(angles[j], &r, &g, &b); + + circle(drawing, contours[i][j], 2, Scalar(b,g,r)); + + int x=drawing.cols-drawing.cols*(j-init_j)/(contours[i].size()-init_j); + line(drawing,Point(x,0), Point(x,10), Scalar(b,g,r)); + } + + #define ANG_SMOOTH 9 + for (j=init_j+ANG_SMOOTH;j=360) ang_diff-=360; + if (ang_diff>=180) ang_diff=360-ang_diff; + + 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; + + double quality = ((double)ang_diff/ANG_SMOOTH) * linear(contours[i][j].y, highy, 1.0, highy+ (drawing.rows-highy)/10, 0.0, true) + * linear( abs(drawing.cols/2 - contours[i][j].x), 0.8*drawing.cols/2, 1.0, drawing.cols/2, 0.6, true); + int y2=25+40+100-5*quality; + + line(drawing, Point(x,y), Point(x,y), Scalar(255,255,255)); + line(drawing, Point(x,25+40+100), Point(x,25+40+100), Scalar(127,127,127)); + line(drawing, Point(x,y2), Point(x,y2), Scalar(255,255,255)); + + circle(drawing, contours[i][j], 2, col); + } + + delete [] angles; + } + } + } + + Point midpoint=Point(drawing.cols/2, 250); + for (int a=0; a<360; a++) + { + double s=sin((double)a*3.141592654/180.0); + double c=cos((double)a*3.141592654/180.0); + 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) ); + } + + + + + area_history_sum-=area_history[area_history_ptr]; + area_history[area_history_ptr]=area_cnt[maxi]; + area_history_sum+=area_cnt[maxi]; + area_history_ptr=(area_history_ptr+1)%AREA_HISTORY; + int prev_area=area_history_sum/AREA_HISTORY; + + cout << "\r\e[2A area = "<=10) + { + cout << "\nALERT: too fast road area change!\n\n\n" << flush; + alertcnt=0; + } + + alertcnt++; + if (alertcnt == 20) cout << "\n\n\n\n\n\n------------------------\n\n\n"; + + imshow("input",thres); + imshow("contours",drawing); + //waitKey(100); + waitKey(); + } + +} diff --git a/mariokart01.cpp b/mariokart01.cpp index b5a5a9b..bc0be6f 100644 --- a/mariokart01.cpp +++ b/mariokart01.cpp @@ -20,11 +20,13 @@ */ - +#include #include #include #include #include +#include +#include #include #include #include @@ -285,7 +287,7 @@ Joystick::~Joystick() void Joystick::steer(float dir, float dead_zone) { if (dir<-1.0) dir=-1.0; - if (dir>1.0) dir=1.0; + if (dir>1.0) dir=1.0; if (fabs(dir)=0.0) ? (powf(b,e)) : (-powf(-b,e)); } + + +sem_t thread1_go; +sem_t thread1_done; +Mat thread1_img; + +void* thread1_func(void*) +{ + Mat gray; + static Mat gray_prev; + + static std::vector points[2]; + + std::vector status; // status of tracked features + std::vector err; // error in tracking + + + cout << "thread 1 is alive :)" < features; // detected features + + // detect the features + goodFeaturesToTrack(gray, // the image + features, // the output detected features + 3000, // the maximum number of features + 0.2, // quality level + 10); // min distance between two features + + + // add the detected features to + // the currently tracked features + points[0].insert(points[0].end(), features.begin(),features.end()); + } + + + // for first image of the sequence + if(gray_prev.empty()) + gray.copyTo(gray_prev); + + cv::calcOpticalFlowPyrLK( + gray_prev, gray, // 2 consecutive images + points[0], // input point positions in first image + points[1], // output point positions in the 2nd image + status, // tracking success + err); // tracking error + + + + int k=0; + for(int i=0; i < points[1].size(); i++) { + // do we keep this point? + if (status[i]) { + // keep this point in vector + points[0][k] = points[0][i]; + points[1][k++] = points[1][i]; + } + } + // eliminate unsuccesful points + points[0].resize(k); + points[1].resize(k); + + + + // for all tracked points + for(int i= 0; i < points[1].size(); i++ ) { + // draw line and circle + cv::line(thread1_img, + points[0][i], // initial position + points[1][i],// new position + cv::Scalar(255,255,255)); + + cv::circle(thread1_img, points[1][i], 2, + cv::Scalar(255,255,255),-1); + } + + // 4. extrapolate movement + for (int i=0;i + +#include + +using namespace std; +using namespace cv; + + + cv::Mat gray; // current gray-level image + cv::Mat gray_prev; // previous gray-level image + // tracked features from 0->1 + std::vector points[2]; + // initial position of tracked points + std::vector initial; + std::vector features; // detected features + int max_count=500; // maximum number of features to detect + double qlevel=0.01; // quality level for feature detection + double minDist=3.3; // min distance between two points + std::vector status; // status of tracked features + std::vector err; // error in tracking + + + +// feature point detection +void detectFeaturePoints() { + // detect the features + cv::goodFeaturesToTrack(gray, // the image + features, // the output detected features + max_count, // the maximum number of features + qlevel, // quality level + minDist); // min distance between two features +} + +// determine if new points should be added +bool addNewPoints() { + // if too few points + return points[0].size()<=300; +} + +// determine which tracked point should be accepted +bool acceptTrackedPoint(int i) { + return status[i];/* && + // if point has moved + (abs(points[0][i].x-points[1][i].x)+ + (abs(points[0][i].y-points[1][i].y))>0);*/ +} + +// handle the currently tracked points +void handleTrackedPoints(cv:: Mat &frame, + cv:: Mat &output) { + // for all tracked points + for(int i= 0; i < points[1].size(); i++ ) { + // draw line and circle + cv::line(output, + points[0][i], // initial position + points[1][i],// new position + cv::Scalar(255,255,255)); + cv::circle(output, points[1][i], 2, + cv::Scalar(255,255,255),-1); + } +} + + +int main(int argc, char** argv) +{ +if (argc<2) {printf("usage: %s videofile [scale]\n",argv[0]); exit(1);} + VideoCapture capture(argv[1]); + if (!capture.isOpened()) + { + cout << "couldn't open file" << endl; + exit(1); + } + +//VideoCapture capture(0); + +// capture.set(CV_CAP_PROP_FRAME_WIDTH, 320); +// capture.set(CV_CAP_PROP_FRAME_HEIGHT, 240); +// capture.set(CV_CAP_PROP_FPS, 15); + +//Mat tmp; for (int i=0;i<1000;i++) capture.read(tmp); + +namedWindow("input"); +namedWindow("output"); + +while(1) +{ + Mat frame; + capture.read(frame); + + Mat output; + + // convert to gray-level image + cv::cvtColor(frame, gray, CV_BGR2GRAY); + frame.copyTo(output); + // 1. if new feature points must be added + if(addNewPoints()) + { + // detect feature points + detectFeaturePoints(); + + + // add the detected features to + // the currently tracked features + points[0].insert(points[0].end(), + features.begin(),features.end()); + initial.insert(initial.end(), + features.begin(),features.end()); + + } + // for first image of the sequence + if(gray_prev.empty()) + gray.copyTo(gray_prev); + // 2. track features + cv::calcOpticalFlowPyrLK( + gray_prev, gray, // 2 consecutive images + points[0], // input point positions in first image + points[1], // output point positions in the 2nd image + status, // tracking success + err); // tracking error + // 2. loop over the tracked points to reject some + int k=0; + for( int i= 0; i < points[1].size(); i++ ) { + // do we keep this point? + if (acceptTrackedPoint(i)) { + // keep this point in vector + initial[k]= initial[i]; + points[0][k] = points[0][i]; + points[1][k++] = points[1][i]; + } + } + // eliminate unsuccesful points + points[0].resize(k); + points[1].resize(k); + initial.resize(k); + // 3. handle the accepted tracked points + handleTrackedPoints(frame, output); + + + //// 4. current points and image become previous ones + //std::swap(points[1], points[0]); + + // 4. extrapolate movement + for (int i=0;i