summaryrefslogtreecommitdiff
path: root/detect_road_borders.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'detect_road_borders.cpp')
-rw-r--r--detect_road_borders.cpp179
1 files changed, 171 insertions, 8 deletions
diff --git a/detect_road_borders.cpp b/detect_road_borders.cpp
index 2e2ad07..5a7c31d 100644
--- a/detect_road_borders.cpp
+++ b/detect_road_borders.cpp
@@ -1,3 +1,5 @@
+//frame 275: da hängts!!
+
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
@@ -7,6 +9,34 @@
using namespace std;
using namespace cv;
+
+int find_intersection_index(int x0, int y0, int x1, int y1, int** contour_map, bool stop_at_endpoint=true) // bresenham aus der dt. wikipedia
+// returns: the point's index where the intersection happened, or a negative number if no intersection.
+{
+ int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
+ int dy = -abs(y1-y0), sy = y0<y1 ? 1 : -1;
+ int err = dx+dy, e2; /* error value e_xy */
+
+ for(;;){ /* loop */
+
+ //setPixel(x0,y0);
+ if (contour_map[x0][y0]>0) return contour_map[x0][y0]; // found intersection?
+ if (contour_map[x0][y0+1]>0) return contour_map[x0][y0+1];
+ if (contour_map[x0+1][y0]>0) return contour_map[x0+1][y0];
+
+
+
+
+ if (stop_at_endpoint && x0==x1 && y0==y1) break;
+ e2 = 2*err;
+ if (e2 > dy) { err += dy; x0 += sx; } /* e_xy+e_x > 0 */
+ if (e2 < dx) { err += dx; y0 += sy; } /* e_xy+e_y < 0 */
+ }
+
+ return -1;
+}
+
+
Mat circle_mat(int radius)
{
Mat result(radius*2+1, radius*2+1, CV_8U);
@@ -119,16 +149,27 @@ int main(int argc, char* argv[])
Mat thres(frame.rows, frame.cols, CV_8UC1);
Mat tmp(frame.rows, frame.cols, CV_8UC1);
-
+ int** contour_map;
+ contour_map=new int*[frame.cols];
+ for (int i=0;i<frame.cols;i++)
+ contour_map[i]=new int[frame.rows];
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;
+ int frameno=0;
while (1)
{
capture >> frame;
+
+ if (frameno<190)
+ {
+ frameno++;
+ continue;
+ }
+
cvtColor(frame, tmp, CV_RGB2GRAY);
threshold(tmp, thres, 132, 255, THRESH_BINARY);
dilate(thres,tmp,Mat());
@@ -240,6 +281,7 @@ int main(int argc, char* argv[])
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 );
@@ -255,7 +297,7 @@ int main(int argc, char* argv[])
for( int i = 0; i< contours.size(); i++ )
{
if (hierarchy[i][3]<0)
- {
+ {
int lowy=0, lowj=-1;
int highy=drawing.rows;
@@ -273,6 +315,12 @@ int main(int argc, char* argv[])
if (lowj!=-1)
{
std::rotate(contours[i].begin(),contours[i].begin()+lowj,contours[i].end());
+
+ // create contour map
+ for (int j=0;j<frame.cols;j++) // zero it
+ memset(contour_map[j],0,frame.rows*sizeof(**contour_map));
+ for (int j=0;j<contours[i].size(); j++) // fill it
+ contour_map[contours[i][j].x][contours[i][j].y]=j;
int j;
for (j=0;j<contours[i].size();j++)
@@ -317,6 +365,7 @@ int main(int argc, char* argv[])
}
#define ANG_SMOOTH 9
+ double* angle_derivative = new double[contours[i].size()];
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);
@@ -332,23 +381,133 @@ int main(int argc, char* argv[])
int y=25+40-2*ang_diff/ANG_SMOOTH;
+ angle_derivative[j] = (double)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;
+ //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));
+ //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);
}
+ for (int j=init_j; j<init_j+ANG_SMOOTH; j++) angle_derivative[j]=angle_derivative[init_j+ANG_SMOOTH];
+ for (int j=contours[i].size()-ANG_SMOOTH; j<contours[i].size(); j++) angle_derivative[j]=angle_derivative[contours[i].size()-ANG_SMOOTH-1];
+
+ double lastmax=-999999;
+ double bestquality=0.0;
+ double bestquality_max=0.0;
+ int bestquality_j=-1;
+ int bestquality_width=-1;
+
+ #define MAX_HYST 0.8
+ for (int j=3;j<contours[i].size()-3;j++)
+ {
+ if (angle_derivative[j] > lastmax) lastmax=angle_derivative[j];
+ if (angle_derivative[j] < MAX_HYST*lastmax && angle_derivative[j+1] < MAX_HYST*lastmax && angle_derivative[j+2] < MAX_HYST*lastmax)
+ {
+ int j0=-1;
+ for (j0=j-1; j0>=0; j0--)
+ if (angle_derivative[j0] < MAX_HYST*lastmax && angle_derivative[j0-1] < MAX_HYST*lastmax && angle_derivative[j0-2] < MAX_HYST*lastmax)
+ break;
+
+ if (lastmax > 5)
+ {
+ // the maximum area goes from j0 to j
+ int x=drawing.cols-drawing.cols*((j+j0)/2-init_j)/(contours[i].size()-init_j);
+
+ double quality = ((double)angle_derivative[(j+j0)/2]) * 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);
+
+ if (quality>bestquality)
+ {
+ bestquality=quality;
+ bestquality_max=lastmax;
+ bestquality_j=(j+j0)/2;
+ bestquality_width=j-j0;
+ }
+
+ line(drawing, Point(x,25+40-3*quality), Point(x, 25+40), Scalar(0,255,0));
+ circle(drawing, contours[i][(j+j0)/2], 1, Scalar(128,0,0));
+ }
+ lastmax=-999999;
+ }
+ }
+
+ circle(drawing, contours[i][bestquality_j], 3, Scalar(255,255,0));
+ circle(drawing, contours[i][bestquality_j], 2, Scalar(255,255,0));
+ circle(drawing, contours[i][bestquality_j], 1, Scalar(255,255,0));
+ circle(drawing, contours[i][bestquality_j], 0, Scalar(255,255,0));
+
+ int antisaturation = 200-(200* bestquality/10.0);
+ if (antisaturation<0) antisaturation=0;
+ for (int j=0;j<bestquality_j-bestquality_width/2;j++)
+ circle(drawing, contours[i][j], 2, Scalar(255,antisaturation,255));
+ for (int j=bestquality_j+bestquality_width/2;j<contours[i].size();j++)
+ circle(drawing, contours[i][j], 2, Scalar(antisaturation,255,antisaturation));
+
+ line(drawing, contours[i][bestquality_j], Point(drawing.cols/2, drawing.rows-drawing.rows/5), Scalar(0,64,64));
+
+ int intersection = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, contours[i][bestquality_j].x, contours[i][bestquality_j].y, contour_map);
+ if (intersection>=0) // should always be true
+ {
+ circle(drawing, contours[i][intersection], 2, Scalar(0,0,0));
+ circle(drawing, contours[i][intersection], 1, Scalar(0,0,0));
+
+ int xx=contours[i][bestquality_j].x;
+ int lastheight=-1;
+ if (intersection < bestquality_j) // im pinken bereich, also zu weit rechts
+ {
+ for (; xx>=0; xx--)
+ {
+ int intersection2 = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contours[i][bestquality_j].y, contour_map);
+ if (intersection2<0)
+ break;
+ if (intersection2>=bestquality_j) // im gegenüberliegenden bereich?
+ {
+ if (contours[i][intersection2].y>=lastheight) xx++; // undo last step
+ break;
+ }
+ lastheight=contours[i][intersection2].y;
+ }
+ }
+ else if (intersection > bestquality_j) // im grünen bereich, also zu weit links
+ {
+ for (; xx<drawing.cols; xx++)
+ {
+ int intersection2 = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contours[i][bestquality_j].y, contour_map);
+ if (intersection2<0)
+ break;
+ if (intersection2<=bestquality_j) // im gegenüberliegenden bereich?
+ {
+ if (contours[i][intersection2].y>=lastheight) xx--; // undo last step
+ break;
+ }
+ lastheight=contours[i][intersection2].y;
+ }
+ }
+ // else // genau den horizontpunkt getroffen
+ // do nothing
+
+ int steering_point = find_intersection_index(drawing.cols/2, drawing.rows-drawing.rows/5, xx, contours[i][bestquality_j].y, contour_map, false);
+ if (steering_point>=0) // should be always true
+ line(drawing, contours[i][steering_point], Point(drawing.cols/2, drawing.rows-drawing.rows/5), Scalar(0,255,255));
+ }
+
+ cout << "bestquality_width="<<bestquality_width <<",\tquality="<<bestquality<<",\t"<<"raw max="<<bestquality_max
+ <<endl<<endl<<endl<<endl;
+
+
+ delete [] angle_derivative;
delete [] angles;
}
}
}
- Point midpoint=Point(drawing.cols/2, 250);
+ /*Point midpoint=Point(drawing.cols/2, 250); // farbkreis
for (int a=0; a<360; a++)
{
double s=sin((double)a*3.141592654/180.0);
@@ -356,7 +515,7 @@ int main(int argc, char* argv[])
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) );
- }
+ }*/
@@ -385,10 +544,14 @@ int main(int argc, char* argv[])
alertcnt++;
if (alertcnt == 20) cout << "\n\n\n\n\n\n------------------------\n\n\n";
+ cout << "frame #"<<frameno<<endl;
+
imshow("input",thres);
imshow("contours",drawing);
- //waitKey(100);
+// waitKey(100);
waitKey();
+
+ frameno++;
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-22 20:33:33 +0000