#include <iostream>
#include <sstream>
#include <string.h>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/video.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/video/tracking.hpp>
#include <cmath>
#include <vector>
using namespace std;
using namespace cv ;
int main()
{
int count;
Mat frame, img, prevImg, temp, gray, img_temp;
VideoCapture cap("04.avi");
//namedWindow("Frame");
cap >> img_temp;
cvtColor(img_temp, gray, CV_BGR2GRAY);
gray.convertTo(temp, CV_8U);
bilateralFilter(temp, prevImg, 5, 20, 20);
double fps = cap.get(CV_CAP_PROP_FPS);
while(true)
{
count=0;
cap >> frame;
cvtColor(frame, img, CV_BGR2GRAY);
int win_size = 3;
int maxCorners = 1000;
double qualityLevel = 0.01;
double minDistance = 1;
int blockSize = 7;
double k = 0.04;
vector<Point2f> img_corners;
img_corners.reserve(maxCorners);
vector<Point2f> prevImg_corners;
prevImg_corners.reserve(maxCorners);
int width = img.cols;
int height = img.rows;
//goodFeaturesToTrack(img, img_corners, maxCorners,qualityLevel,minDistance,Mat(),blockSize,true,k);
int g_size = 10;
Point2f points;
for(int i =0; i< width/g_size;++i){
for(int j =0; j< width/g_size;++j){
points.x = i* g_size + g_size/2;
points.y = j* g_size + g_size/2;
}
img_corners.push_back(points);
}

/*cornerSubPix( img, img_corners, Size( win_size, win_size ), Size( -1, -1 ),
TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.03 ) );
*/
vector<uchar> features_found;
features_found.reserve(maxCorners);
vector<float> feature_errors;
feature_errors.reserve(maxCorners);
calcOpticalFlowPyrLK( img, prevImg, img_corners, prevImg_corners, features_found, feature_errors ,
Size( win_size, win_size ), 3,
cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.3 ), 0, k);
for( int i=0; i < features_found.size(); i++ ){
Point2f p0( ceil( img_corners[i].x ), ceil( img_corners[i].y ) );
Point2f p1( ceil( prevImg_corners[i].x ), ceil( prevImg_corners[i].y ) );
line( frame, p0, p1, CV_RGB(255,0,0), 5 );
}
imshow("frame", frame);
if(waitKey(5) >= 0)
break;
}
}