detect_road_borders geaddet

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.);
+}
+
+}