/ 光流物件的跟蹤.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));
	}
}