lenk- und strassenerkennungskram in klassen ausgelagert. funzt.

10 files changed, 405 insertions, 245 deletions

diff --git a/Makefile b/Makefile
index 6cd4d1f..98b4d9c 100644
--- a/Makefile
+++ b/Makefile
@@ -3,15 +3,27 @@ all: detect_road_borders mariokart01
 detect_road_borders: detect_road_borders.cpp
 	g++ `pkg-config --libs --cflags opencv` -g $< -o $@
 
+road_thresholder.o: road_thresholder.cpp road_thresholder.h
+	g++ `pkg-config --cflags opencv` -g road_thresholder.cpp -c
+
 horizon_steerer.o: horizon_steerer.cpp horizon_steerer.h
-	g++ `pkg-config --libs --cflags opencv` -g horizon_steerer.cpp -c
+	g++ `pkg-config --cflags opencv` -g horizon_steerer.cpp -c
+
+naive_steerer.o: naive_steerer.cpp naive_steerer.h
+	g++ `pkg-config --cflags opencv` -g naive_steerer.cpp -c
 
 steer_accumulator.o: steer_accumulator.cpp steer_accumulator.h
-	g++ `pkg-config --libs --cflags opencv` -g steer_accumulator.cpp -c
+	g++ `pkg-config --cflags opencv` -g steer_accumulator.cpp -c
+
+util.o: util.cpp util.h
+	g++ `pkg-config --cflags opencv` -g util.cpp -c
 
 test_detect: detect_road_borders
 	./detect_road_borders test.mpg
 
-mariokart01: mariokart01.cpp
-	g++ `pkg-config --libs --cflags opencv` -lxcb -lpthread -g $< -o $@
+mariokart01.o: mariokart01.cpp
+	g++ `pkg-config --cflags opencv` -lxcb -lpthread -g mariokart01.cpp -c
+
+mariokart01: mariokart01.o road_thresholder.o horizon_steerer.o naive_steerer.o steer_accumulator.o util.o
+	g++ `pkg-config --libs --cflags opencv` -lxcb -lpthread -g mariokart01.o road_thresholder.o horizon_steerer.o naive_steerer.o steer_accumulator.o util.o -o $@
 
diff --git a/horizon_steerer.h b/horizon_steerer.h
index fba4209..738ab7d 100644
--- a/horizon_steerer.h
+++ b/horizon_steerer.h
@@ -6,11 +6,11 @@
 
 using namespace cv;
 
-class HorizonSteerer : SteerIface
+class HorizonSteerer : public SteerIface
 {
 public:
 	HorizonSteerer(int xlen_, int ylen_);
-	virtual ~HorizonSteerer();
+	virtual ~HorizonSteerer() {};
 	int find_ideal_line(vector<Point>& contour, Point origin_point, int** contour_map, int bestquality_j);
 
 private:
diff --git a/mariokart01.cpp b/mariokart01.cpp
index 8a8b39d..dc6eaa9 100644
--- a/mariokart01.cpp
+++ b/mariokart01.cpp
@@ -20,8 +20,8 @@
  */
 
 
-//#define FREEBSD
-#define LINUX
+#define FREEBSD
+//#define LINUX
 
 #include <vector>
 #include <unistd.h>
@@ -42,6 +42,11 @@
 #include <sys/stat.h>
 #include <sys/uio.h>
 
+#include "util.h"
+#include "steer_interface.h"
+#include "naive_steerer.h"
+#include "road_thresholder_iface.h"
+#include "road_thresholder.h"
 
 #include <iostream>
 #include <list>
@@ -492,25 +497,6 @@ void Joystick::process()
 #define IMG_HISTORY 3
 
 
-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;
-}
 
 
 
@@ -526,12 +512,6 @@ void mouse_callback(int event, int x, int y, int flags, void* userdata)
 }
 
 
-float flopow(float b, float e)
-{
-	return (b>=0.0) ? (powf(b,e)) : (-powf(-b,e));
-}
-
-
 
 
 
@@ -708,8 +688,6 @@ joystick.reset();
   XorgGrabber capture("Mupen64Plus OpenGL Video");
 #endif
 
-  int road_0=77, road_1=77, road_2=77;
-  
   Mat transform;
   
   bool first=true;
@@ -730,29 +708,20 @@ joystick.reset();
   
   VideoWriter img_writer, thres_writer, thres2_writer;
   
-  
-  Mat img_history[IMG_HISTORY];
-  int history_pointer=0;
-  
-  while(1)
-  {
-    
-  Mat img_;
-  
-    capture.read(img_);
-  
-  if (first)
-  {
-    xlen=img_.cols;
-    ylen=img_.rows;
-    
-    crosshair_x=xlen/2;
-    crosshair_y=0.58*ylen;
-    
-    Point2f src_pts[4] =  { Point2f(0, ylen), Point2f(xlen, ylen),        Point2f(xlen* (.5 - 0.13), ylen/2), Point2f(xlen* (.5 + .13), ylen/2) };
-    //Point2f dest_pts[4] = { Point2f(0, ylen), Point2f(trans_width, ylen), Point2f(0, trans_height),       Point2f(0, trans_height) };
-    Point2f dest_pts[4] = { Point2f(trans_width/2 - road_width/2, trans_height), Point2f(trans_width/2 + road_width/2, trans_height),        Point2f(trans_width/2 - road_width/2, trans_height/2), Point2f(trans_width/2 + road_width/2, trans_height/2) };
-    transform=getPerspectiveTransform(src_pts, dest_pts);
+	Mat img_;
+
+	capture.read(img_);
+
+	xlen=img_.cols;
+	ylen=img_.rows;
+
+	crosshair_x=xlen/2;
+	crosshair_y=0.58*ylen;
+
+	Point2f src_pts[4] =  { Point2f(0, ylen), Point2f(xlen, ylen),        Point2f(xlen* (.5 - 0.13), ylen/2), Point2f(xlen* (.5 + .13), ylen/2) };
+	//Point2f dest_pts[4] = { Point2f(0, ylen), Point2f(trans_width, ylen), Point2f(0, trans_height),       Point2f(0, trans_height) };
+	Point2f dest_pts[4] = { Point2f(trans_width/2 - road_width/2, trans_height), Point2f(trans_width/2 + road_width/2, trans_height),        Point2f(trans_width/2 - road_width/2, trans_height/2), Point2f(trans_width/2 + road_width/2, trans_height/2) };
+	transform=getPerspectiveTransform(src_pts, dest_pts);
 
 	//img_writer.open("img.mpg", CV_FOURCC('P','I','M','1'), 30, Size(xlen,ylen), false);
 	img_writer.open("img.mpg", CV_FOURCC('P','I','M','1'), 30, Size(xlen,ylen), false);
@@ -764,22 +733,27 @@ joystick.reset();
 		cout << "ERROR: could not open video files!" << !img_writer.isOpened() << !thres_writer.isOpened() << !thres2_writer.isOpened() <<endl;
 	}
 
-    first=false;
-  }
+  
+  SteerIface* steerer = new NaiveSteerer(xlen/2, 0.58*ylen);
+  RoadThresholderIface* road_thresholder = new RoadThresholder();
+  
+  while(1)
+  {
+    capture.read(img_);
+  
   assert ((img_.cols==xlen) && (img_.rows==ylen));
     
-  Mat img, img2;
+  Mat img;
   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);
-  for (int row = 0; row<img2.rows; row++)
+  //assert(img.type()==CV_8UC3);
+  for (int row = 0; row<img.rows; row++)
   {
-    uchar* data=img2.ptr<uchar>(row);
+    uchar* data=img.ptr<uchar>(row);
     
-    for (int col=0; col<img2.cols;col++)
+    for (int col=0; col<img.cols;col++)
     {
       int sum=data[0] + data[1] + data[2];
       if (sum>0)
@@ -794,109 +768,27 @@ joystick.reset();
         data[1]=255/3;
         data[2]=255/3;
       }
-      data+=img2.step[1];
+      data+=img.step[1];
     }
   }
   #endif
   
   
-  Mat img_diff(img2.rows, img2.cols, CV_8U);
-  int hist[256];
-  for (int i=0; i<256; i++) hist[i]=0;
-  for (int row = 0; row<img2.rows; row++)
-  {
-    uchar* data=img2.ptr<uchar>(row);
-    uchar* data_out=img_diff.ptr<uchar>(row);
-    
-    for (int col=0; col<img2.cols;col++)
-    {
-      int diff = (abs((int)data[0] - road_0) + abs((int)data[1] - road_1) + abs((int)data[2] - road_2)) /3;
-      if (diff>255) { cout << "error, diff is" << diff << endl; diff=255; }
-      *data_out=diff;
-      hist[diff]++;
-      data+=img2.step[1];
-      data_out++;
-    }
-  }
-  
-  int hist2[256];
-  for (int i=0; i<256; i++)
-  {
-    int sum=0;
-    for (int j=-HIST_SMOOTH; j<=HIST_SMOOTH; j++)
-    {
-      if (i+j < 0 || i+j > 255) continue;
-      sum+=hist[i+j];
-    }
-    hist2[i]=sum;
-  }
-  
-  int cumul=0;
-  int x_begin=0;
-  for (x_begin=0;x_begin<256;x_begin++)
-  {
-    cumul+=hist[x_begin];
-    if (cumul > img2.rows*img2.cols/100) break;
-  }
-  
-  int hist_max=0;
-  int thres;
-  for (int i=0; i<256; i++)
-  {
-    if (hist2[i]>hist_max) hist_max=hist2[i];
-    if ((hist2[i] < hist_max/2) && (i>x_begin))
-    {
-      thres=i;
-      break;
-    }
-  }
-  
-  //thres-=thres/4;
-  
-  
-  Mat img_hist(100,256, CV_8U);
-  for (int row = 0; row<img_hist.rows; row++)
-  {
-    uchar* data=img_hist.ptr<uchar>(row);
-    
-    for (int col=0; col<img_hist.cols;col++)
-    {
-      *data=255;
-      if (col==thres) *data=127;
-      if (col==x_begin) *data=0;
-      if (hist2[col] > (img_hist.rows-row)*800) *data=0;
-      data++;
-    }
-  }
-  
-  Mat img_thres(img_diff.rows, img_diff.cols, img_diff.type());
-  threshold(img_diff, img_thres, thres, 255, THRESH_BINARY_INV);
+  road_thresholder->process_image(img);
+  Mat img_thres = road_thresholder->get_road();
   
   Mat img_eroded(img_thres.rows, img_thres.cols, img_thres.type());
   Mat img_tmp(img_thres.rows, img_thres.cols, img_thres.type());
   Mat img_thres2(img_thres.rows, img_thres.cols, img_thres.type());
-  Mat img_stddev(img_thres.rows, img_thres.cols, img_thres.type());
-  img_stddev=Mat::zeros(img_thres.rows, img_thres.cols,  img_thres.type());
   
   erode(img_thres, img_tmp, erode_2d_small);
-  img_tmp.copyTo(img_eroded);
   dilate(img_tmp, img_thres, erode_2d_small);
-
   dilate(img_thres, img_tmp, erode_2d_big);
   erode(img_tmp, img_thres2, erode_2d_big);
 
 
 
 
-  img_thres.copyTo(img_history[history_pointer]);
-  
-  Mat historized=img_history[0]/IMG_HISTORY;
-  for (int i=1; i<IMG_HISTORY; i++)
-    if (historized.cols == img_history[i].cols)
-	  historized+=img_history[i]/IMG_HISTORY;
-	
-  history_pointer=(history_pointer+1)%IMG_HISTORY;
-
 
 
 
@@ -905,85 +797,10 @@ joystick.reset();
   
   assert(img.rows==img_eroded.rows);
   assert(img.cols==img_eroded.cols);
-  int avg_sum=0;
-  int r0=0, r1=0, r2=0;
-  int left_sum=0, right_sum=0;
-  for (int row = 0; row<img2.rows; row++)
-  {
-    uchar* data=img2.ptr<uchar>(row);
-    uchar* mask=img_eroded.ptr<uchar>(row);
-    
-    int mean_value_line_sum=0;
-    int mean_value_line_cnt=0;
-    int mean_value_line;
-    for (int col=0; col<img2.cols;col++)
-    {
-      if (*mask)
-      {
-		if (row<=crosshair_y)
-		{
-			if (col < crosshair_x)
-				left_sum++;
-			else
-				right_sum++;
-		}
-		  
-        avg_sum++;
-        mean_value_line_sum+=col;
-        mean_value_line_cnt++;
-        r0+=data[0];
-        r1+=data[1];
-        r2+=data[2];
-      }
-      
-      data+=img.step[1];
-      mask++;
-    }
+
     
-    if (mean_value_line_cnt)
-    {
-		mean_value_line=mean_value_line_sum/mean_value_line_cnt;
-		int variance_line=0;
-		int stddev_line;
-		uchar* mask=img_eroded.ptr<uchar>(row);
-		data=img2.ptr<uchar>(row);
-		for (int col=0; col<img2.cols;col++)
-		{
-		  if (*mask)
-			variance_line+=(col-mean_value_line)*(col-mean_value_line);
-		  
-		  data+=img2.step[1];
-		  mask++;
-		}
-		variance_line/=mean_value_line_cnt;
-		stddev_line=sqrt(variance_line);
-		if (mean_value_line>1 && mean_value_line < img2.cols-2)
-		{
-			img_stddev.ptr<uchar>(row)[mean_value_line-1]=255;
-			img_stddev.ptr<uchar>(row)[mean_value_line]=255;
-			img_stddev.ptr<uchar>(row)[mean_value_line+1]=255;
-		}
-		if ((mean_value_line+stddev_line)>1 && (mean_value_line+stddev_line) < img2.cols-2)
-		{
-			img_stddev.ptr<uchar>(row)[mean_value_line-1+stddev_line]=255;
-			img_stddev.ptr<uchar>(row)[mean_value_line+stddev_line]=255;
-			img_stddev.ptr<uchar>(row)[mean_value_line+1+stddev_line]=255;
-		}
-		if ((mean_value_line-stddev_line)>1 && (mean_value_line-stddev_line) < img2.cols-2)
-		{
-			img_stddev.ptr<uchar>(row)[mean_value_line-1-stddev_line]=255;
-			img_stddev.ptr<uchar>(row)[mean_value_line-stddev_line]=255;
-			img_stddev.ptr<uchar>(row)[mean_value_line+1-stddev_line]=255;
-		}
-	}
-  }
   
-  if (avg_sum>20)
-  {
-	  road_0=r0/avg_sum;
-	  road_1=r1/avg_sum;
-	  road_2=r2/avg_sum;
-	}
+
 
 /*
   Mat img_perspective(trans_height, trans_width, img_thres.type());
@@ -999,20 +816,19 @@ joystick.reset();
   */
 
   
-  img2.row(crosshair_y)=Scalar(255,0,0);
-  img2.col(crosshair_x)=Scalar(255,0,0);
-  img_diff.row(crosshair_y)=255;
-  img_diff.col(crosshair_x)=255;
+  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;
-  img_stddev.row(crosshair_y)=255;
-  img_stddev.col(crosshair_x)=255;
+
+	steerer->process_image(img_thres2);
+	double steer_value = steerer->get_steer_data();
 
   Mat steer=Mat::zeros(20,1920,CV_8U);
   steer.col( steer.cols /2 )=128;
-  if (left_sum+right_sum>0)
+  if (steerer->get_confidence()>0)
   {
-	  int x = (steer.cols-2) * left_sum / (left_sum+right_sum)    +1;
+	  int x = (steer_value/2.0 + 0.5)*steer.cols;
 	  
 	  if (x<1) x=1;
 	  if (x>=steer.cols-1) x=steer.cols-2;
@@ -1022,7 +838,7 @@ joystick.reset();
 	  steer.col(x+1)=240;
 	  
 	  
-	  joystick.steer(- 4* flopow(  (((float)left_sum / (left_sum+right_sum))-0.5  )*2.0 , 1.6)  ,0.05);
+	  joystick.steer( steer_value,0.05);
   }
   else
     joystick.steer(0.0);
@@ -1034,23 +850,18 @@ joystick.reset();
 
 
   //imshow("orig", img);
-  imshow("edit", img2);
+  imshow("edit", img);
   //imshow("perspective", img_perspective);
   //imshow("diff", img_diff);
-  imshow("hist", img_hist);
+  
   imshow("thres", img_thres);
   imshow("thres2", img_thres2);
   imshow("tracked", thread1_img);
-  //imshow("history", historized);
-  //imshow("stddev", img_stddev);
   imshow("steer", steer);
   
-  Mat road_color(100,100, CV_8UC3, Scalar(road_0, road_1, road_2));
-  imshow("road_color", road_color);
-
 
 
-  img_writer << img_diff;
+  //img_writer << img_diff;
   thres_writer << img_thres;
   thres2_writer << img_thres2;
 
diff --git a/naive_steerer.cpp b/naive_steerer.cpp
new file mode 100644
index 0000000..9844664
--- /dev/null
+++ b/naive_steerer.cpp
@@ -0,0 +1,53 @@
+#include "naive_steerer.h"
+#include "util.h"
+
+using namespace std;
+
+NaiveSteerer::NaiveSteerer(int chx, int chy)
+{
+	set_crosshair(chx,chy);
+}
+
+void NaiveSteerer::set_crosshair(int chx, int chy)
+{
+	crosshair_x=chx;
+	crosshair_y=chy;
+}
+
+void NaiveSteerer::process_image(const Mat& img)
+{
+	int left_sum=0, right_sum=0;
+	for (int row = 0; row<img.rows; row++)
+	{
+		const uchar* data=img.ptr<uchar>(row);
+		
+		for (int col=0; col<img.cols;col++)
+		{
+			if (*data)
+			{
+				if (row<=crosshair_y)
+				{
+					if (col < crosshair_x)
+						left_sum++;
+					else
+						right_sum++;
+				}
+			}
+			
+			data+=img.step[1];
+		}
+	}
+	
+	steer = -4* flopow(  (((float)left_sum / (left_sum+right_sum))-0.5  )*2.0 , 1.6);
+}
+
+double NaiveSteerer::get_steer_data()
+{
+	return steer;
+}
+
+double NaiveSteerer::get_confidence()
+{
+	//return confidence;
+	return 1.0; // TODO
+}
diff --git a/naive_steerer.h b/naive_steerer.h
new file mode 100644
index 0000000..d31582a
--- /dev/null
+++ b/naive_steerer.h
@@ -0,0 +1,30 @@
+#ifndef __NAIVE_STEERER_H__
+#define __NAIVE_STEERER_H__
+
+#include <opencv2/opencv.hpp>
+#include "steer_interface.h"
+#include <list>
+
+using namespace cv;
+
+
+class NaiveSteerer : public SteerIface
+{
+	public:
+		NaiveSteerer(int chx, int chy);
+		void set_crosshair(int chx, int chy);
+		
+		virtual ~NaiveSteerer() {};
+		
+		virtual void process_image(const Mat& img);
+		virtual double get_steer_data();
+		virtual double get_confidence();
+	
+	private:
+		double steer;
+		double confidence;
+		int crosshair_x;
+		int crosshair_y;
+};
+
+#endif
diff --git a/road_thresholder.cpp b/road_thresholder.cpp
new file mode 100644
index 0000000..344c9b3
--- /dev/null
+++ b/road_thresholder.cpp
@@ -0,0 +1,167 @@
+#include <opencv2/opencv.hpp>
+#include "road_thresholder.h"
+#include "util.h"
+
+using namespace cv;
+
+RoadThresholder::RoadThresholder()
+{
+	road_0=77;
+	road_1=77;
+	road_2=77;
+	
+	erode_2d_small = circle_mat(3);
+}
+RoadThresholder::RoadThresholder(int r0, int r1, int r2)
+{
+	road_0=r0;
+	road_1=r1;
+	road_2=r2;
+	
+	erode_2d_small = circle_mat(3);
+}
+
+Mat RoadThresholder::create_diff_image_and_fill_histogram(const Mat& img, int* hist)
+{
+	Mat img_diff(img.rows, img.cols, CV_8U);
+	for (int i=0; i<256; i++) hist[i]=0; // zero the histogram
+	
+	// iterate through every pixel of img
+	for (int row=0; row<img.rows; row++)
+	{
+		const uchar* data=img.ptr<uchar>(row);
+		uchar* data_out=img_diff.ptr<uchar>(row);
+		
+		for (int col=0; col<img.cols;col++)
+		{
+			int diff = (abs((int)data[0] - road_0) + abs((int)data[1] - road_1) + abs((int)data[2] - road_2)) /3;
+			if (diff>255) { printf("error, diff is %i\n",diff); diff=255; }
+			
+			*data_out=diff;
+			hist[diff]++;
+			
+			data+=img.step[1];
+			data_out++;
+		}
+	}
+	
+	return img_diff;
+}
+
+void RoadThresholder::smoothen_histogram(int* hist, int* hist_smooth, int smoothness)
+{
+	for (int i=0; i<256; i++)
+	{
+		int sum=0;
+		for (int j=-smoothness; j<=smoothness; j++)
+		{
+			if (i+j < 0 || i+j > 255) continue;
+			sum+=hist[i+j];
+		}
+		hist_smooth[i]=sum;
+	}
+}
+
+
+void RoadThresholder::calc_road_color(const Mat& img, const Mat& mask_eroded)
+{
+	int avg_sum=0;
+	int r0=0, r1=0, r2=0;
+	for (int row = 0; row<img.rows; row++)
+	{
+		const uchar* data=img.ptr<uchar>(row);
+		const uchar* mask=mask_eroded.ptr<uchar>(row);
+		
+		for (int col=0; col<img.cols;col++)
+		{
+			if (*mask)
+			{
+				avg_sum++;
+				r0+=data[0];
+				r1+=data[1];
+				r2+=data[2];
+			}
+			
+			data+=img.step[1];
+			mask++;
+		}
+	}
+	
+	if (avg_sum>20)
+	{
+		road_0=r0/avg_sum;
+		road_1=r1/avg_sum;
+		road_2=r2/avg_sum;
+	}
+}
+
+void RoadThresholder::process_image(const Mat& img)
+{
+	int hist_unsmoothened[256];
+	Mat img_diff = create_diff_image_and_fill_histogram(img, hist_unsmoothened);
+	
+	int hist_smoothened[256];
+	smoothen_histogram(hist_unsmoothened, hist_smoothened, 7);
+	
+	// road has to occupy at least 1% of the image
+	int cumul=0;
+	int x_begin=0;
+	for (x_begin=0;x_begin<256;x_begin++)
+	{
+		cumul+=hist_unsmoothened[x_begin];
+		if (cumul > img.rows*img.cols/100) break;
+	}
+	
+	// find first valley in the histogram, and set the threshold at the way down.
+	int hist_max=0;
+	int thres=0;
+	for (int i=0; i<256; i++)
+	{
+		if (hist_smoothened[i]>hist_max) hist_max=hist_smoothened[i];
+		if ((hist_smoothened[i] < hist_max/2) && (i>x_begin))
+		{
+			thres=i;
+			break;
+		}
+	}
+	
+
+	// drawing stuff
+	Mat img_hist(100,256, CV_8U);
+	for (int row = 0; row<img_hist.rows; row++)
+	{
+		uchar* data=img_hist.ptr<uchar>(row);
+		
+		for (int col=0; col<img_hist.cols;col++)
+		{
+			*data=255;
+			if (col==thres) *data=127;
+			if (col==x_begin) *data=0;
+			if (hist_smoothened[col] > (img_hist.rows-row)*800) *data=0;
+			data++;
+		}
+	}
+	imshow("hist", img_hist);
+	
+	
+	mask_raw = Mat(img_diff.rows, img_diff.cols, img_diff.type());
+	threshold(img_diff, mask_raw, thres, 255, THRESH_BINARY_INV);
+	
+	
+	
+	
+	
+	
+	
+	Mat mask_eroded(mask_raw.rows, mask_raw.cols, mask_raw.type());
+	erode(mask_raw, mask_eroded, erode_2d_small);
+
+	calc_road_color(img, mask_eroded); // update our road color
+	
+	
+}
+
+Mat& RoadThresholder::get_road()
+{
+	return mask_raw;
+}
diff --git a/road_thresholder.h b/road_thresholder.h
new file mode 100644
index 0000000..3ae2361
--- /dev/null
+++ b/road_thresholder.h
@@ -0,0 +1,31 @@
+#ifndef __ROAD_THRESHOLDER_H__
+#define __ROAD_THRESHOLDER_H__
+
+#include <opencv2/opencv.hpp>
+#include "road_thresholder_iface.h"
+
+using namespace cv;
+
+class RoadThresholder : public RoadThresholderIface
+{
+	public:
+		RoadThresholder();
+		RoadThresholder(int r, int g, int b);
+		virtual void process_image(const Mat& img);
+		virtual Mat& get_road();
+	
+	private:
+		Mat mask_raw;
+		int road_0;
+		int road_1;
+		int road_2;
+		Mat erode_2d_small;
+		
+		Mat create_diff_image_and_fill_histogram(const Mat& img, int* histogram);
+		void calc_road_color(const Mat& img, const Mat& mask_eroded);
+		static void smoothen_histogram(int* hist, int* hist_smooth, int smoothness);
+		
+};
+
+
+#endif
diff --git a/road_thresholder_iface.h b/road_thresholder_iface.h
new file mode 100644
index 0000000..2205ca8
--- /dev/null
+++ b/road_thresholder_iface.h
@@ -0,0 +1,16 @@
+#ifndef __ROAD_THRESHOLDER_IFACE_H__
+#define __ROAD_THRESHOLDER_IFACE_H__
+
+#include <opencv2/opencv.hpp>
+
+using namespace cv;
+
+class RoadThresholderIface
+{
+	public:
+		virtual void process_image(const Mat& img)=0;
+		virtual Mat& get_road()=0;
+};
+
+
+#endif
diff --git a/util.cpp b/util.cpp
new file mode 100644
index 0000000..69faa31
--- /dev/null
+++ b/util.cpp
@@ -0,0 +1,27 @@
+#include "util.h"
+
+float flopow(float b, float e)
+{
+	return (b>=0.0) ? (powf(b,e)) : (-powf(-b,e));
+}
+
+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;
+}
+
diff --git a/util.h b/util.h
new file mode 100644
index 0000000..046617f
--- /dev/null
+++ b/util.h
@@ -0,0 +1,13 @@
+#ifndef __UTIL_H__
+#define __UTIL_H__
+
+#include <opencv2/opencv.hpp>
+
+using namespace cv;
+
+float flopow(float b, float e);
+
+Mat circle_mat(int radius);
+
+
+#endif