detect_road_borders geaddet

1 files changed, 172 insertions, 0 deletions

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