/ 光流对象的跟踪.cpp: 定义控制台应用程序的入口点。
//分为稀疏光流 KLT  和稠密光流HF
//本课程用KLT

#include<opencv2/opencv.hpp>
#include<iostream>
using namespace cv;
using namespace std;

Mat frame, gray;
Mat pre_frame, pre_gray;
vector<Point2f>features;//Tomas角点检测特征点
vector<Point2f>inpoints;//初始化特征点数据
vector<Point2f>fpts[2];//保持当前帧和前一帧特征点位置

vector<uchar> status;//特征点各跟踪成功标志位
vector<float> errors;// 跟踪时候区域误差和
void detectcorner(Mat &inframe,Mat &ingray);//角点检测
void drawcorner(Mat &inframe);//角点显示
void track();//角点跟踪
void drawtracklines();
int main(int argc, char ** argv)
{
	VideoCapture capture;
	capture.open(0);
	if (!capture.isOpened())
	{
		printf("can not open ......\n");
		return -1;
	}
	namedWindow("output", WINDOW_AUTOSIZE);

	while (capture.read(frame))
	{
		cvtColor(frame, gray, CV_BGR2GRAY);//光流跟踪需要灰度图像
		if (fpts[0].size() < 40) //跟踪40个特征点，如果跟踪的时候损失了一些特征点，重新检测，追加
		{
			detectcorner(frame, gray);//调用Tomas角点检测
			fpts[0].insert(fpts[0].end(), features.begin(), features.end());// 追加带跟踪的特征点
			inpoints.insert(inpoints.end(), features.begin(), features.end());
		}
		else
		{
			cout << "track.......\n";// 表示特征点没有损失，一直在跟踪

		}
		if (pre_gray.empty())
		{
			gray.copyTo(pre_gray); // 保存前一帧，第一帧过完就不保存了

		}
		track();//角点跟踪，KLT
		drawcorner(frame);//角点显示


		//更新前一帧数据
		gray.copyTo(pre_gray);
		frame.copyTo(pre_frame);

		imshow("output", frame);
		waitKey(1);
	}
	capture.release();//释放内存
	waitKey(0);
	return 0;
}

//角点检测
void detectcorner(Mat &inframe, Mat &ingray)
{
	double maxCorners = 5000;//检测到的角点的数量的最大值
	double  qualilevel = 0.01;//检测到的角点的质量等级，角点特征值小于qualityLevel*最大特征值的点将被舍弃
	double mindistant = 10;//两个角点间最小间距
	double bsize = 3;//计算协方差矩阵时窗口大小
	goodFeaturesToTrack(ingray, features, maxCorners, qualilevel, mindistant, Mat(), bsize, false, 0.04);//Tomas角点检测
	//输入必须为灰度图像，features保存检测到的角点坐标。Mat()检测整幅图像。true使用Harris角点检测，为false，则使用Shi-Tomasi算子
	//留给Harris角点检测算子用的中间参数，一般取经验值0.04~0.06。第八个参数为false时，该参数不起作用；
	//稀疏光流也用到了相邻三层金字塔，只是使用默认值没有参数显示
	cout << "detect" << features.size()<<endl;
}
//角点显示 
void drawcorner(Mat &inframe) {
	for (int i = 0; i < fpts[0].size(); i++)
	{
		circle(inframe, fpts[0][i], 2,Scalar(0, 0, 255));
	}
}

void track()
{
	//输入前衣服图像和当前图像。输入前一副图像特性点，如果在后一副图像追踪到了前一副图像的特征点，则fpts[1]保存该特征点坐标
	//status如果相应位置的流特征被发现，向量的每个元素被设置为1，否则，被置为0.
	calcOpticalFlowPyrLK(pre_gray, gray, fpts[0], fpts[1], status, errors);//KLT,稀疏光流
	int k = 0;// 保存跟踪到的特征点数
	//特征点过滤
	for (int i = 0; i < fpts[1].size(); i++)
	{
		double dist = abs(fpts[0][i].x - fpts[1][i].x) + abs(fpts[0][i].y - fpts[1][i].y);
		if (dist > 2 && status[i])// 跟踪到的特征点，且距离移动了2以上的
		{
			inpoints[k] = inpoints[i];// 将跟踪到的移动了的特征点在vector中连续起来，剔掉损失的和静止不动的特征点(这些跟踪点在前面帧中)
			fpts[1][k++] = fpts[1][i];// 同上 (只是这些跟踪点在当前帧中)
		}
	}
	//保存特征点并绘制跟踪轨迹
	inpoints.resize(k);//重新设置容器大小
	fpts[1].resize(k);//重新设置容器大小
	drawtracklines();//画线
	std::swap(fpts[1], fpts[0]);// 交换，将此帧跟踪到的特征点作为下一帧的待跟踪点
}

void drawtracklines()
{
	for (int t = 0; t < fpts[1].size(); t++)
	{
		line(frame, inpoints[t], fpts[1][t], Scalar(0, 255, 0), 2, 8, 0);//画线
		circle(frame, fpts[1][t], 2, Scalar(0, 0, 255));
	}
}