diff options
| -rw-r--r-- | detect_road_borders.cpp | 394 | ||||
| -rw-r--r-- | mariokart01.cpp | 128 | ||||
| -rw-r--r-- | track_street.cpp | 172 |
3 files changed, 690 insertions, 4 deletions
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 <stdlib.h> +#include <stdio.h> +#include <iostream> +#include <math.h> +#include <opencv2/opencv.hpp> + +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<unsigned char>(result.cols/2 + x, result.rows/2 + y); + unsigned char& p2 = result.at<unsigned char>(result.cols/2 - x, result.rows/2 + y); + unsigned char& p3 = result.at<unsigned char>(result.cols/2 + x, result.rows/2 - y); + unsigned char& p4 = result.at<unsigned char>(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<clipmin) return clipmin; + else return result; + } + else + return result; +} + + +#define AREA_HISTORY 10 +int alertcnt=21; +int main(int argc, char* argv[]) +{ + if (argc!=2) {printf("usage: %s videofile\n",argv[0]); exit(1);} + VideoCapture capture(argv[1]); + + if (!capture.isOpened()) + { + cout << "couldn't open file" << endl; + exit(1); + } + + Mat erode_kernel=circle_mat(10); + + + Mat frame; + capture >> 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<AREA_HISTORY;i++) area_history[i]=1; + int area_history_ptr=0; + int area_history_sum=AREA_HISTORY; + cout << endl<<endl<<endl; + while (1) + { + capture >> 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<thres.rows; row++) + { + uchar* data=thres.ptr<uchar>(row); + + for (int col=0; col<thres.cols;col++) + { + if (*data==255) + { + floodFill(thres, Point(col,row), area_index); + area_index++; + } + + data++; + } + } + + + int* area_cnt = new int[area_index-1]; + int total_area_cnt=0; + for (int i=0;i<area_index-1;i++) area_cnt[i]=0; + + for (int row = 0; row<thres.rows; row++) + { + uchar* data=thres.ptr<uchar>(row); + + for (int col=0; col<thres.cols;col++) + { + if (*data) + { + area_cnt[*data-1]++; + total_area_cnt++; + } + + data++; + } + } + + + /* + // Das ist nur zum schönsein. man wird einfach den größten area_cnt nehmen wollen und den rest nullen. + for (int row = 0; row<thres.rows; row++) + { + uchar* data=thres.ptr<uchar>(row); + + for (int col=0; col<thres.cols;col++) + { + if (*data) + { + long long tmp = (long long )30000*(long)area_cnt[*data-1]/(long)total_area_cnt + 64; + if (tmp>200) 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;i<area_index-1;i++) + { + if (area_cnt[i]>maxa) + { + maxa=area_cnt[i]; + maxi2=maxi; + maxi=i; + } + } + + + // lösche alle bis auf die größte fläche + for (int row = 0; row<thres.rows; row++) + { + uchar* data=thres.ptr<uchar>(row); + + for (int col=0; col<thres.cols;col++) + { + if (*data) + { + if (*data!=maxi+1) *data=0; else *data=255; + } + data++; + } + } + + + + + dilate(thres, tmp, erode_kernel); + erode(tmp, thres, erode_kernel); + + + + Mat thres_tmp, thres_tmp_; + thres.copyTo(thres_tmp); + //thres_tmp=thres_tmp_.rowRange(0, 0.6*thres_tmp_.rows ); + + vector<vector<Point> > contours; + vector<Vec4i> 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<contours[i].size(); j++) + { + if (contours[i][j].y > 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();j++) + if (contours[i][j].y < contours[i][0].y-1) break; + for (;j<contours[i].size();j++) + circle(drawing, contours[i][j], 2, Scalar( 0,255-( j *255/contours[i].size()),( j *255/contours[i].size()))); + + line(drawing, Point(0,highy), Point(drawing.cols,highy), Scalar(127,127,127)); + + #define SMOOTHEN_BOTTOM 25 + #define SMOOTHEN_MIDDLE 10 + + + for (j=0;j<contours[i].size();j++) + if (contours[i][j].y < contours[i][0].y-1) break; + + int init_j=j; + + double* angles = new double[contours[i].size()]; + + for (;j<contours[i].size();j++) + { + int smoothen; + if (contours[i][j].y - thres.rows/2 < 0) + smoothen=SMOOTHEN_MIDDLE; + else + smoothen= SMOOTHEN_MIDDLE + (SMOOTHEN_BOTTOM-SMOOTHEN_MIDDLE) * (contours[i][j].y - thres.rows/2) / (thres.rows/2); + + int j1=(j+smoothen); if (j1 >= 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<contours[i].size()-ANG_SMOOTH;j++) + { + int x=drawing.cols-drawing.cols*(j-init_j)/(contours[i].size()-init_j); + double ang_diff = angles[j+ANG_SMOOTH]-angles[j-ANG_SMOOTH]; + + while (ang_diff<0) ang_diff+=360; + while (ang_diff>=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 = "<<area_cnt[maxi]<<", \tvize = "<<((maxi2!=-1)?area_cnt[maxi2]:-1)<<", \tratio="<< ((maxi2!=-1)?(area_cnt[maxi]/area_cnt[maxi2]):-1 )<<" \n" << + "prev area = "<<prev_area<<",\tchange="<< (100*area_cnt[maxi]/prev_area -100) <<"%\n"<<flush; + + if (maxi!=-1 && maxi2!=-1 && (area_cnt[maxi]/area_cnt[maxi2]) < 10) + { + cout << "\nALERT: possibly split road!\n\n\n" << flush; + alertcnt=0; + } + + if (abs(100*area_cnt[maxi]/prev_area -100) >=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 <vector> #include <unistd.h> #include <stdio.h> #include <stdlib.h> #include <string.h> +#include <pthread.h> +#include <semaphore.h> #include <fcntl.h> #include <linux/input.h> #include <linux/uinput.h> @@ -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)<dead_zone) dir=0.0; @@ -393,11 +395,121 @@ float flopow(float b, float e) return (b>=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<cv::Point2f> points[2]; + + std::vector<uchar> status; // status of tracked features + std::vector<float> err; // error in tracking + + + cout << "thread 1 is alive :)" <<endl; + while(1) + { + sem_wait(&thread1_go); + sem_post(&thread1_done); + + sem_wait(&thread1_go); + // now we have our private image at thread1_img. + + cvtColor(thread1_img, gray, CV_BGR2GRAY); + + + if (points[0].size() <= 2000) // we need more points + { + std::vector<cv::Point2f> 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<points[0].size();i++) + { + points[0][i].x = points[1][i].x+(points[1][i].x-points[0][i].x); + points[0][i].y = points[1][i].y+(points[1][i].y-points[0][i].y); + } + + cv::swap(gray_prev, gray); + + + // now we must stop accessing thread1_img, main() may access it + sem_post(&thread1_done); + } +} + + int main(int argc, char* argv[]) { try { + if (sem_init(&thread1_go, 0, 0)) throw string("sem_init failed"); + if (sem_init(&thread1_done, 0, 0)) throw string("sem_init failed"); + + pthread_t thread1; + if (pthread_create(&thread1, NULL, thread1_func, NULL)) throw string("pthread_create failed"); + + string tmp; Joystick joystick; @@ -507,6 +619,7 @@ try { Mat img, img2; img_.convertTo(img, CV_8UC3, 1); //FINDMICH img.copyTo(img2); + img.copyTo(thread1_img); sem_post(&thread1_go); #ifdef NO_BRIGHTNESS //assert(img2.type()==CV_8UC3); @@ -757,7 +870,7 @@ try { steer.col(x+1)=240; - joystick.steer(- 5* flopow( (((float)left_sum / (left_sum+right_sum))-0.5 )*2.0 , 1.1) ,0.05); + joystick.steer(- 4* flopow( (((float)left_sum / (left_sum+right_sum))-0.5 )*2.0 , 1.6) ,0.05); } else joystick.steer(0.0); @@ -765,6 +878,9 @@ try { + sem_wait(&thread1_done); // wait for thread1 to finish + + //imshow("orig", img); imshow("edit", img2); //imshow("perspective", img_perspective); @@ -772,7 +888,8 @@ try { imshow("hist", img_hist); imshow("thres", img_thres); imshow("thres2", img_thres2); - imshow("history", historized); + imshow("tracked", thread1_img); + //imshow("history", historized); //imshow("stddev", img_stddev); imshow("steer", steer); @@ -799,4 +916,7 @@ catch(...) cout << "error!" << endl; } +sem_destroy(&thread1_go); +sem_destroy(&thread1_done); + } diff --git a/track_street.cpp b/track_street.cpp new file mode 100644 index 0000000..4153068 --- /dev/null +++ b/track_street.cpp @@ -0,0 +1,172 @@ +#include <iostream> + +#include <opencv2/opencv.hpp> + +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<cv::Point2f> points[2]; + // initial position of tracked points + std::vector<cv::Point2f> initial; + std::vector<cv::Point2f> 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<uchar> status; // status of tracked features + std::vector<float> 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<points[0].size();i++) + { + points[0][i].x = points[1][i].x+(points[1][i].x-points[0][i].x); + points[0][i].y = points[1][i].y+(points[1][i].y-points[0][i].y); + } + + + + cv::swap(gray_prev, gray); + + + if (argc==3) + { + Mat out2; + + pyrUp(output, out2, Size(output.cols*2, output.rows*2)); + + imshow("input",frame); + imshow("output", out2); + } + else +{ + imshow("input",frame); + imshow("output", output); +} + + waitKey(1000/30.); +} + +} |