summaryrefslogtreecommitdiff
path: root/track_street.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'track_street.cpp')
-rw-r--r--track_street.cpp172
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.);
+}
+
+}