简单实现一下光流法

之前看了光流法，可以用来目标检测和跟踪，甚至图像拼接，就计划实现一下。
主要实现金字塔光流法。光流法由三大严格假设：
1.亮度恒定不变。
2.小运动。
3.空间一致性。
以上三个假设决定了光流法的局限性，但是光流法在静态、小位移、光不强的情况下，有不俗的表现。此外，金字塔LK光流法也可以勉强容忍较大的位移。特征点多选用健壮的角点特征。
此方法仅作为验证金字塔光流法的用途，耗时很长。
此外，Mat::at<>(i，j) 方法实在是太耗费时间了，我懒得改了，如果想节省时间，能用指针，就别用函数。详情可以看这篇文章，其实光流法是十分快速的。
https://blog.csdn.net/wanggao_1990/article/details/53264754
at和ptr有很大差别，用ptr取出来的数永远都是整型，at取出来的是float类型，我怀疑是编写人员当时没注意这个问题。

#define MAX 1000000000

#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/videoio.hpp"
#include <iostream>
#include <deque>
//#include <math.h>

using namespace cv;
using namespace std;

struct max_p
{
	int max;
	Point match[2];//0q 1h
	max_p()
	{
		max = MAX;
		match[0].x = 0;
		match[0].y = 0;
		match[1].x = 0;
		match[1].y = 0;
	}
};


vector<Point> Harris(const Mat &ori,int th)
{
	Mat har = Mat::zeros(ori.size(), CV_32F);
	vector<Point> result;
	cornerHarris(ori, har, 2, 3, 0.05);
	normalize(har, har, 0, 255, NORM_MINMAX, CV_8U, Mat());
	convertScaleAbs(har,har);
	for (int j = 0; j <har.rows; j++)
	{
		for (int i = 0; i < har.cols; i++)
		{
			if (har.at<uchar>(j, i) > th)
			{
				result.push_back(Point(i, j));
			}
		}
	}
	return result;
}
void track(const Mat& no_move, const Mat& move, vector<Point> &res, vector<Point> match_p,int quick=1)
{
	if (no_move.size() != move.size())
		return ;
	int dx[5] = { -2,-1,0,1,2 };
	int dy[5] = { -2,-1,0,1,2 };


	for (int n = 0; n < res.size(); n++)
	{
		int min = MAX;
		//match_p.push_back(Point(0, 0));
		if (res[n].x <= 2 || res[n].y <= 2 || res[n].x >= move.cols - 2 || res[n].y >= move.rows - 2)
			continue;

		for (int j = match_p[n].y - move.rows*0.05; j < match_p[n].y + move.rows*0.05; j++)
		{
			for (int i = match_p[n].x - move.cols*0.05; i < match_p[n].x + move.cols*0.05; i++)
			{
				if (j <= 2 || i <= 2 || j >= move.rows - 2 || i >= move.cols - 2)
					continue;


				int ess = 0;
				for (int k = 0; k < 5; k++)
				{
					for (int m = 0; m < 5; m++)
					{
						int temp_v = no_move.at<uchar>(res[n].y + dx[k], res[n].x + dy[m]) - move.at<uchar>(j + dx[k], i + dy[m]);
						ess += temp_v * temp_v;
					}
				}
				//ess = sqrt(ess);
				if (ess < min)
				{
					min = ess;
					match_p.pop_back();
					match_p.push_back(Point(i, j));
				}
				else
				{
					i += quick;
				}

			}
		}
	}
}
vector<Point> match(const Mat& no_move,const Mat& move,vector<Point> &res,int quick=0)
{
	vector<Point> match_p;
	if (no_move.size() != move.size())
		return match_p;
	int dx[5] = {-2,-1,0,1,2};
	int dy[5]= {-2,-1,0,1,2};
	
	
		for (int n = 0; n < res.size(); n++)
		{
			int min = MAX;
			match_p.push_back(Point(0, 0));
			if (res[n].x <= 2 || res[n].y <= 2 || res[n].x >= move.cols - 2 || res[n].y >= move.rows - 2)
				continue;

			for (int j =  0; j < move.rows; j++)
			{
				for (int i =  0; i < move.cols; i++)
				{
					if (j <= 2 || i <= 2 || j >= move.rows - 2 || i >= move.cols - 2)
						continue;


					int ess = 0;
					for (int k = 0; k < 5; k++)
					{
						for (int m = 0; m < 5; m++)
						{
							int temp_v = no_move.at<uchar>(res[n].y+ dx[k], res[n].x + dy[m]) - move.at<uchar>(j + dx[k], i + dy[m]);
							ess += temp_v * temp_v;
						}
					}
					//ess = sqrt(ess);
					if (ess < min)
					{
						min = ess;
						match_p.pop_back();
						match_p.push_back(Point(i, j));
					}
					else
					{
						i += quick;
					}

				}
			}
		}

	
	return match_p;
}

void draw_circle(Mat &a,vector<Point> &p)
{
	for (int i = 0; i < p.size(); i++)
	{
		circle(a, p[i], 1, Scalar(0), 2, 8, 0);
	}
}

void x2(vector<Point> &a)
{
	for (int i = 0; i < a.size(); i++)
	{
		a[i].x *= 2;
		a[i].y *= 2;
	}
}

void draw_line(Mat &pic,vector<Point>&a, vector<Point>&b)
{
	for (int i = 0; i < a.size(); i++)
	{
		line(pic, a[i], b[i], Scalar(125),2);
	}
}
int main()
{
	Mat img= imread("5.jpg", 0);
	Mat img1;
	Mat img2;
	Mat img3;
	resize(img, img1, img.size() / 2);
	resize(img1, img2, img1.size() / 2);
	resize(img2, img3, img2.size() / 2);
	Mat move= imread("6.jpg", 0);
	Mat move1;
	Mat move2;
	Mat move3;
	resize(move, move1, move.size() / 2);
	resize(move1, move2, move1.size() / 2);
	resize(move2, move3, move2.size() / 2);
	//int quick = 3;//加速项
	vector<Point> a = Harris(move3,150);
	vector<Point> b = match(move3, img3, a);
	x2(a);
	x2(b);
	track(move2, img2, a, b);
	x2(a);
	x2(b);
	track(move1, img1, a, b);
	x2(a);
	x2(b);
	track(move, img, a, b);
	draw_circle(move, a);
	draw_circle(img, b);
	draw_line(img, a, b);
	imshow("move2", move);
	imshow("img2", img);
	

	waitKey();
}