From 8775126ba9391d6e9eca61d5746dcf80cafa19f2 Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Mon, 3 Dec 2012 23:24:27 +0100 Subject: lenk- und strassenerkennungskram in klassen ausgelagert. funzt. --- Makefile | 20 +++- horizon_steerer.h | 4 +- mariokart01.cpp | 289 ++++++++--------------------------------------- naive_steerer.cpp | 53 +++++++++ naive_steerer.h | 30 +++++ road_thresholder.cpp | 167 +++++++++++++++++++++++++++ road_thresholder.h | 31 +++++ road_thresholder_iface.h | 16 +++ util.cpp | 27 +++++ util.h | 13 +++ 10 files changed, 405 insertions(+), 245 deletions(-) create mode 100644 naive_steerer.cpp create mode 100644 naive_steerer.h create mode 100644 road_thresholder.cpp create mode 100644 road_thresholder.h create mode 100644 road_thresholder_iface.h create mode 100644 util.cpp create mode 100644 util.h 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& 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 #include @@ -42,6 +42,11 @@ #include #include +#include "util.h" +#include "steer_interface.h" +#include "naive_steerer.h" +#include "road_thresholder_iface.h" +#include "road_thresholder.h" #include #include @@ -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(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; -} @@ -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() <(row); + uchar* data=img.ptr(row); - for (int col=0; col0) @@ -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(row); - uchar* data_out=img_diff.ptr(row); - - for (int col=0; col255) { 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(row); - - for (int col=0; 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(row); - uchar* mask=img_eroded.ptr(row); - - int mean_value_line_sum=0; - int mean_value_line_cnt=0; - int mean_value_line; - for (int col=0; col(row); - data=img2.ptr(row); - for (int col=0; col1 && mean_value_line < img2.cols-2) - { - img_stddev.ptr(row)[mean_value_line-1]=255; - img_stddev.ptr(row)[mean_value_line]=255; - img_stddev.ptr(row)[mean_value_line+1]=255; - } - if ((mean_value_line+stddev_line)>1 && (mean_value_line+stddev_line) < img2.cols-2) - { - img_stddev.ptr(row)[mean_value_line-1+stddev_line]=255; - img_stddev.ptr(row)[mean_value_line+stddev_line]=255; - img_stddev.ptr(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(row)[mean_value_line-1-stddev_line]=255; - img_stddev.ptr(row)[mean_value_line-stddev_line]=255; - img_stddev.ptr(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(row); + + for (int col=0; col +#include "steer_interface.h" +#include + +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 +#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(row); + uchar* data_out=img_diff.ptr(row); + + for (int col=0; col255) { 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(row); + const uchar* mask=mask_eroded.ptr(row); + + for (int col=0; col20) + { + 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(row); + + for (int col=0; 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 +#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 + +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(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; +} + 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 + +using namespace cv; + +float flopow(float b, float e); + +Mat circle_mat(int radius); + + +#endif -- cgit v1.2.1