稀疏光流KLT跟踪算法

1.前言

前段时间学习了Harris角点检测和Shi-Tomasi角点检测，但实际用途没用，特此记录一下运用角点的稀疏光流KLT跟踪算法.

2.概念

2.1光流概念

光流是空间运动物体在观察成像平面上的像素运动的瞬时速度，是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息的一种方法。

之前在知乎上看到关于光流特别像的水流解释。水流是水的流动，光流是发光,反射光的物体的移动，对应到一张张图像就是图像中像素的移动。

我们为什么要研究光流。
我们研究光流都是通过摄像机和设想物体之间相对运动产生的，在物体所成的图像序列中，可以观测到的亮度模式的明显变化称光流。
所以这里有一个运动场和光流场的概念。运动场的定义:摄影机和空间环境存在相对运动;图像中的每-一个点都对应于一个空间点;每一个空间点相对于摄影机的速度向量都可以投影为像平面上一个点上的二维速度向量，图像上每个像素的二维速度向量就构成运动场。

简单的来说，运动场反应的是现实世界中的运动，是摄像机观察到的运动。光流场是摄像机获得的图像中（运动场在二维图像中）的投影

此图片很好理解，就是世界三维速度矢量转化成图像中二维的速度。

现在普遍的以计算光流的方法有三个方向：
基于区域或者基于特征的匹配方法；
基于频域的方法；
基于梯度的方法；

稀疏光流跟踪算法对运用场景对有着三个要求：
1.亮度恒定：像素在运动过程中亮度（灰度值）恒定。（这就意味着我们在处理图像需要把图像转换为灰度图像）
2.短距离移动：在检测过程中，光流在两帧之间（连续处理的两张图片中）不能有过大移动。
3.空间一致性：当前帧的图像中相邻的像素点在下一帧也应是相邻的

算法数学原理
以梯度的方法为例，求光流约束方程。
假设一帧图像中的像素对于$t$时刻在$（x,y）$，下一帧图像中的像素对于$t+∆t$时刻在
$(x+△x,y+∆y)$。
对于亮度一致性假设，可得：$I(x,y,t)=I(x+△x,y+∆y,t+∆t)$
（$I(x,y,t)$表示在时间 $t$ 时刻像素 $(x,y)$点的光强度）

对于下一帧图像中的光强度进行泰勒一阶展开，可得：
$I(x+△x,y+∆y,t+∆t)=I(x,y,z)+△x+∆y+∆t+\epsilon$，其中$\epsilon$为无穷小，式子可转化成
$I(x+△x,y+∆y,t+∆t)=I(x,y,z)+\frac{\partial I}{\partial x}△x+\frac{\partial I}{\partial y}∆y+\frac{\partial I}{\partial t}∆t$
由于亮度一致性知:$I(x,y,t)=I(x+△x,y+∆y,t+∆t)$
可推出：$\frac{\partial I}{\partial x}△x+\frac{\partial I}{\partial y}∆y+\frac{\partial I}{\partial t}∆t=0$

两边再同时除以$\partial t$,得
$\frac{\partial I}{\partial x}\frac{\partial x}{\partial t}+\frac{\partial I}{\partial y}\frac{\partial y}{\partial t}+\frac{\partial I}{\partial t}\frac{\partial t}{\partial t}=\frac{\partial I}{\partial x}\frac{\partial x}{\partial t}+\frac{\partial I}{\partial y}\frac{\partial y}{\partial t}+\frac{\partial I}{\partial t}=0$

如图所示，设$u,v$分别为光流沿着x轴方向和y轴方向的速度矢量，则可将$u,v$表示为：$u=\frac{dx}{dy},v=\frac{dy}{dt}$

设$I_x,I_y$分别表示图像中像素点沿$x,y$方向的图像梯度，$I_t$为图像灰度对时间的变化量
$I_x=\frac{\partial I}{\partial x},I_y=\frac{\partial I}{\partial y},I_t=\frac{\partial I}{\partial t}$

所以我们可以从
$\frac{\partial I}{\partial x}\frac{\partial x}{\partial t}+\frac{\partial I}{\partial y}\frac{\partial y}{\partial t}+\frac{\partial I}{\partial t}=0$
转化成
$I_{x}u+I_{y}v+I_t=0$

我们可以得到光流方程：$I_{x}u+I_{y}v=-I_t$

化为矩阵形式为:
$$\left(\begin{array}{c}{I}_x\\ I_y\end{array}\right)\cdot \left(\begin{array}{c}u\\ v\end{array}\right)=-I_t$$

在光流方程中，有 $u,v$ 两个未知数，一个方程，两个未知数。
由此图知，沿着灰度梯度方向的光流分量可以确定，但是垂直于灰度梯度方向.上的光流分量无法确定。

2.2稀疏光流KLT跟踪算法概念

上面已经求得了光流方程，但还有两个未知量需要求解。KLT算法就是求解$(u,v)$两个未知量。

根据之前要求的是三个假设，使用需要求解的像素周围5×5的像素块来帮组我们得到更多的方程式：
$$\left[\begin{array}{c}{I}_{x1}{I}_{y1}\\ I_{x1}{I}_{y1}\\ I_{x1}{I}_{y1}\\ \cdots \\ I_{x}25I_{y}25\end{array}\right]\left[\begin{array}{c}\partial x\\ \partial y\end{array}\right]=\left[\begin{array}{c}-I_{t1}\\ -I_{t2}\\ \cdots \\ -I_{t24}\\ -I_{t24}\end{array}\right]$$
从矩阵化成方程组就可以得到超定方程组，也就是方程个数，使用最小二乘法求解方程组。
设$A\left[\begin{array}{c}{I}_{x1}{I}_{y1}\\ I_{x1}{I}_{y1}\\ I_{x1}{I}_{y1}\\ \cdots \\ I_{x}25I_{y}25\end{array}\right],b=\left[\begin{array}{c}{I}_{t1}\\ I_{t2}\\ \cdots \\ I_{t24}\\ I_{t24}\end{array}\right]$,
所以上面式子可化成：$A\left[\begin{array}{c}\partial x\\ \partial y\end{array}\right]=-b$
设$d=\left[\begin{array}{c}\partial x\\ \partial y\end{array}\right]$,则我们可以得到：$Ad=-b$
同时乘以$A^T$得：
$A^{T}Ad=-A^{T}b$
$d=-(A^{T}A{)}^{-1}{A}^{T}b$
d是使用最小二乘法求解的。

KLT光流等式为：
$\left[\begin{array}{cc}\sum I_x{I}_x& \sum I_x{I}_y\\ \sum I_x{I}_y& \sum I_y{I}_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=\left[\begin{array}{c}\sum I_x{I}_t\\ \sum I_y{I}_t\end{array}\right]$

hessian矩阵
$M=A^{T}A=\left[\begin{array}{cc}\sum I_x{I}_x& \sum I_x{I}_y\\ \sum I_x{I}_y& \sum I_y{I}_y\end{array}\right]$

通过上述数学运算我们可知方程的解d需要满足：
1.$A^{T}A$必须是可逆的
2.$A^{T}A$ 中的特征值${\lambda }_1$和${\lambda }_2$不能太小。当特征值过小，说明像素块选在平缓区域（特征值太小，就不会判定为角点）。
3.$A^{T}A$ 中 ${\lambda }_1/{\lambda }_2$不能太大。当 ${\lambda }_1/{\lambda }_2$ 过大时，说明素块选在边缘区域。

我们如何判断通过特征值来判断所处的矩阵是角点，边缘，还是平缓区域呢？

我这里以下列图片为例，

所以我们明白了，如何选择角点。接下来需要明白我们运用摄像机捕获图像时，目标大幅度的运动会打破一致的运动的假设，我们需要运用到图像金字塔。

高斯金字塔-用来对图像进行降采样（用前一层的运动估计值作为下一层估计运动的起始点。 从顶层开始计算，直到算到底层（原图）。）

3.代码实现

代码为C++语言编写，调用摄像头实现角点光流跟踪。

#include<iostream>
#include<opencv2/opencv.hpp>

using namespace cv;
using namespace std;

vector<Point2f>featurePoints;
RNG rng(5000);

void draw_goodFeatures(Mat &image, vector<Point2f> goodFeatures);//定义绘制角点函数
void draw_lines(Mat &image, vector<Point2f> pt1, vector<Point2f> pt2);//定义绘制跟踪线函数

vector<Scalar> color_lut;

int main()
{
	VideoCapture cap(1);//笔记本默认0，外接一个摄像头为1

	if (!cap.isOpened())//判断是否寻找到摄像头
	{
		cout << "Video read error..." << endl;
		return -1;
	}
	Mat frame,gray_frame;
	//设置摄像头的参数
	cap.set(CV_CAP_PROP_FRAME_WIDTH, 960);//宽度
	cap.set(CV_CAP_PROP_FRAME_HEIGHT, 640);//高度

	//初始化goodFeatursToTrack的参数
	int maxCorners = 6000;
	double qualityLevel = 0.01;
	double minDistance = 10;
	int blockSize = 3;
	bool useHarrisDetector = false;
	double k = 0.04;

	//声明vector容器和初始化光流KLT跟踪算法的参数
	vector<Point2f>pts[2];//定义两个vector数组，一个为pts[0]，一个为pts[1]
	vector<uchar>status;//存储KLT光流的输出状态
	vector<float>err;//存储错误
	int maxLevel = 3;//金字塔的层数，默认为3
	// 窗口搜索时候停止条件
	TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01); 

	int flags = 0; // 操作标志
	double derivlambda = 0.5;

	Mat old_frame, old_gray_frame;//创建前一帧的参数
	vector<Point2f> initPoints;//存储需要跟踪的特征点(静止点会被删除)
	cap.read(old_frame);
	cvtColor(old_frame, old_gray_frame, COLOR_BGR2GRAY);
	//Shi-Tomasi角点检测
	goodFeaturesToTrack(old_gray_frame, featurePoints, maxCorners, qualityLevel, minDistance, Mat(), blockSize, useHarrisDetector, k);
	initPoints.insert(initPoints.end(), featurePoints.begin(), featurePoints.end());//将旧图像的角点存储
	
	//将检测到的角点插入到pts[0]容器中
	pts[0].insert(pts[0].end(), featurePoints.begin(), featurePoints.end());

	int width = cap.get(CAP_PROP_FRAME_WIDTH);//获得视频的宽度
	int height = cap.get(CAP_PROP_FRAME_HEIGHT);//获得视频的高度

	Mat result = Mat::zeros(Size(width * 2, height), CV_8UC3);//创建Mat存储一帧图像作为循环中上一帧的图像

	Rect roi(0, 0, width, height);//存储矩形视频框的坐标(0,0)，和高度、宽度
	cout << "进入循环" << endl;
	
	while (true)
	{
		bool ret = cap.read(frame);
		if (!ret)
		{
			break;
		}
		
		imshow("frame", frame);
		//waitKey(60);//给予渲染时间

		roi.x = 0;//设置检测x轴坐标
		frame.copyTo(result(roi));
		cvtColor(frame, gray_frame, COLOR_BGR2GRAY);//将这一帧图像转化成灰度图像
		//计算光流
		calcOpticalFlowPyrLK(old_gray_frame, gray_frame, pts[0], pts[1], status, err, Size(31, 31), maxLevel, criteria, derivlambda, flags);
		
		size_t i, k;
		for ( i = 0, k = 0; i < pts[1].size(); i++)
		{
			// 距离与状态测量(上一帧的坐标-下一帧的坐标)
			double dist = abs(pts[0][i].x - pts[1][i].x) + abs(pts[0][i].y - pts[1][i].y);
			//如果状态量为1且每次移动到下一帧的距离大于2，就会画跟踪线
			if (status[i] && dist>2)
			{
				pts[0][k] = pts[0][i];
				initPoints[k] = initPoints[i];//这里需要在赋值后马上存储到有用特诊点，否则运行会出现矢量下标超出范围
				pts[1][k++] = pts[1][i];

				circle(frame, pts[1][i], 3, Scalar(0, 255, 255), -1, 8);	
			}
		}
		//重新设置 有用的特征点
		pts[0].resize(k);
		pts[1].resize(k);
		initPoints.resize(k);
		draw_lines(frame, initPoints, pts[1]);
		imshow("result", frame);
		roi.x = width;
		frame.copyTo(result(roi));
		char c = waitKey(50);
		if (c == 27)break;//按Esc退出

		//更新数据
		std::swap(pts[1], pts[0]);//将新一帧图像数据作为旧一帧图像数据，继续进入循环
		cv::swap(old_gray_frame, gray_frame);

		if (initPoints.size() < 40) {
			goodFeaturesToTrack(old_gray_frame, featurePoints, maxCorners, qualityLevel, minDistance, Mat(), blockSize, useHarrisDetector, k);
			initPoints.insert(initPoints.end(), featurePoints.begin(), featurePoints.end());//将重新寻找到的角点插入特诊点容器中，重新循环跟踪
			pts[0].insert(pts[0].end(), featurePoints.begin(), featurePoints.end());
			//cout << "总特诊点：" << pts[0].size() << endl;
		}
		//writer.write(result);
	}
	return 0;
}

//实现绘制角点
void draw_goodFeatures(Mat &image, vector<Point2f>goodFeatures){
	for (size_t t = 0; t < goodFeatures.size(); t++)
	{
		circle(image, goodFeatures[t],2, Scalar(255, 0, 255), 2, 8, 0);
	}
}


//绘制跟踪线
void draw_lines(Mat &image, vector<Point2f>pt1, vector<Point2f>pt2)
{
	//对随机颜色的拾取
	if (color_lut.size() < pt1.size()) {
		for (size_t t = 0; t < pt1.size(); t++)
		{
			color_lut.push_back(Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)));
		}
	}
	//循环绘制跟踪线
	for (size_t t = 0; t < pt1.size(); t++)
	{
		line(image, pt1[t], pt2[t], color_lut[t], 2, 8, 0);
	}
}

效果图：

总结

稀疏光流KLT跟踪算法不是特别难，主要是对角点检测的运用，算法部分唯一难点就是求光流约束方程和通过特征值来判断所处位置。