视觉SLAM前端——LK光流法

目录：

• LK光流介绍
• 单层LK光流
• 多层LK光流

LK光流

  LK光流是一种描述图像运动的方法，利用LK光流可以实现对图像的追踪，从而求解图像运动的位姿。其基本思想如下：

  img1,img2分别为两张已知的图像，相机在运动过程中，img1中的特征点$P(u,v)$经过变换后得到了img2中的$P^{\prime }(u+\Delta x,v+\Delta y)$。

  LK光流法求解相机位姿的基本问题可以描述为：已知$P$点，求解最佳的$\Delta x,\Delta y$，从而得到img2中的$P^{\prime }$坐标。因此便求得一对匹配点$P-P^{\prime }$。在求得多对匹配点的坐标后，整个位姿估计问题可以转换为$ICP$或$PnP$问题。

  为了求解最佳的$\Delta x,\Delta y$，通常采用最小二乘法。在LK光流中有两条非常重要的假设：

1.灰度不变假设，即$I(u,v)=I(u+\Delta x,v+\Delta y)$
2.小范围灰度不变假设，即存在$W\times W$的窗口，使$I(u+w,v+w)=I(u+\Delta x+w,v+\Delta y+w)$
  假设1是基础，假设2则是为了构建最小二乘解。

单层光流法

求解的基本流程如下：

• 特征点的选取
• Gauss-Newton法求解$\Delta x,\Delta y$

1.特征点的选取
  关于特征点的选取，笔者前面写了一篇文章进行了详细介绍，这里不在赘述。详情可点击此处.

vector<KeyPoint> kp1;
	vector<KeyPoint> kp2_single;
	Ptr<GFTTDetector> detector = cv::GFTTDetector::create(500, 0.01, 20);
	detector->detect(img1, kp1);

  这里使用的是GFFT特征。

2.Gauss-Newton求解$\Delta x,\Delta y$

  根据假设1有：$error=I_1(u,v)-I_2(u+\Delta x,v+\Delta y)$,这是基于假设1所得到了，理论上error应该为0，由于噪声的存在，实际上不可能为0，因此只能求解最小二乘解。

  根据假设2有$Cost={\sum }_{j=1}^w{\sum }_{i=1}^w{I}_1(u+w_{,}v+w)-I_2(u+\Delta x+w,v+\Delta y+w)$。

  导数$J$是img2的像素梯度，也可以用img1的像素梯度代替，此时为反向光流法。具体形式可参考《视觉SLAM十四讲》。

代码如下：

void OpticalFlowTracker::calculateOpticalFlow(const Range &range) { //参数是一个cv::range的引用


// parameters                                      range是一个区间，应该看作一个窗口
	//第一步：初始化工作：定义全局变量
	int iteration = 15;  //迭代次数
	int max_patch_size = 4;  //w的大小
	for (int i = range.start; i < range.end; i++)  //初始化变量工作
	{
		auto kp = kp1[i];
		double dx = 0, dy = 0;
		Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
		Eigen::Vector2d b = Eigen::Vector2d::Zero();
		Eigen::Vector2d J;
		double cost = 0;
		double lastcost = 0;
		bool succ = true;
		if (has_initial)   //金字塔的核心
		{
			dx = kp2[i].pt.x - kp.pt.x;
			dy = kp2[i].pt.y - kp.pt.y;  //这个值的加减顺序不能变，
			

		} //初始化或者不初始化都可以

		//第二部：计算e,cost,h,b,j
		for (int iter = 0; iter < iteration; iter++)
		{
			if (inverse == false) {
				H = Eigen::Matrix2d::Zero();
				b = Eigen::Vector2d::Zero();
			}
			else {
				// only reset b
				b = Eigen::Vector2d::Zero();
			}
			cost = 0;

           // 两个for循环为w窗口的迭代
			for (int x = -max_patch_size; x < max_patch_size; x++)
			{
				for (int y = -max_patch_size; y < max_patch_size; y++)
				{
					double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
						GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;

					if (inverse == false)   //inverse是反向的意思
					{

						J = -1.0 * Eigen::Vector2d(  //正向梯度，img2
							0.5*(GetPixelValue(img2, kp.pt.x + x + dx + 1, kp.pt.y + y + dy) -
								GetPixelValue(img2, kp.pt.x + x + dx - 1, kp.pt.y + y + dy)),
							0.5*(GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy + 1) -
								GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
						);

					}
					else if (iter == 0)
					{
						J = -1.0 * Eigen::Vector2d(  //反向梯度，img1
							0.5*(GetPixelValue(img1, kp.pt.x + x + dx + 1, kp.pt.y + y + dy) -
								GetPixelValue(img1, kp.pt.x + x + dx - 1, kp.pt.y + y + dy)),
							0.5*(GetPixelValue(img1, kp.pt.x + x + dx, kp.pt.y + y + dy + 1) -
								GetPixelValue(img1, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
						);

					}

					cost += error * error;
					b += -J * error;

					if (iter == 0 || inverse == false)
					{
						H += J * J.transpose();
					}

				}

			}

			//第三部分：第i次迭代完毕，开始求解/

			Eigen::Vector2d update = H.ldlt().solve(b);
			//求解后不要急于迭代，先看看结果是否正确

			if (std::isnan(update[0])) {
				// sometimes occurred when we have a black or white patch and H is irreversible
				cout << "update is nan" << endl;//判断是不是为无穷大,
				succ = false;
				break;
			}

			if (iter > 0 && cost > lastcost) {  //判断损失函数的增长方向
				break;
			}

			lastcost = cost;
			dx += update(0, 0);
			dy += update(1, 0);
			succ = true;//这一句放在这可以保证每次迭代都不出问题

			if (update.norm() < 1e-2) {
				// converge
				break;
			}



		}
		success[i] = succ;

		//把特征点放到kp2里面  注意：keypoint.pt返回的是Point2f而不是2d；
		kp2[i].pt = kp.pt + Point2f(dx, dy);  //把结果返回

	}
}

多层光流法

  多层光流法的基本思想是在单层的基础上，多次运用单层光流法，同时保存每层的求解结果并作为下一层的初始值。这样可以逐层求解，得到比较实用的光流法。其中，最关键的工具应该是金字塔。

  图像金字塔其实是在原图像的基础上，对图像进行缩放，当然，各参数也要进行相应的缩放。一般顶层图像最粗糙，底层为原图，求解过程由上至下，求解结果逐步精确。多层LK光流法的流程如下：

• 定义金字塔：一般是定义容器
• 创建金字塔：一般是利用循环以及resize函数，给容器赋值
• 求解：逐层调用单层的求解算法，但要注意各层之间的结果是如何传递的。

代码如下：

void OpticalFlowMultiLevel(
	const Mat &img1,
	const Mat &img2,
	const vector<KeyPoint> &kp1,
	vector<KeyPoint> &kp2,
	vector<bool> &success,
	bool inverse) {

	//定义全局参数
	int pyramid_size = 4;
	double pyramid_scal[] = { 1.0,0.5,0.25,0.125 };
	double scal = 0.5;

	//创建金字塔：第0层为底层，第3层为顶层
	vector<Mat> img1_pyramid, img2_pyramid;
	for (int i = 0; i < pyramid_size; i++)
	{
		if (i == 0) //第0层为原图
		{
			img1_pyramid.push_back(img1);
			img2_pyramid.push_back(img2);
		}
		else
		{
			Mat pyramid1, pyramid2; //利用好resize函数
			resize(img1_pyramid[i - 1], pyramid1,
				cv::Size(img1_pyramid[i - 1].cols*scal, img1_pyramid[i - 1].rows*scal));
			img1_pyramid.push_back(pyramid1);

			resize(img2_pyramid[i - 1], pyramid2,
				cv::Size(img2_pyramid[i - 1].cols*scal, img2_pyramid[i - 1].rows*scal));
			img2_pyramid.push_back(pyramid2); //图像金字塔仅仅作为导数的计算用
		}
	}
	//同理，坐标也要创建为金字塔
	vector<KeyPoint> kp1_pyramid, kp2_pyramid;
	for (auto kp_top : kp1)
	{
		kp_top.pt *= pyramid_scal[pyramid_size - 1];
		kp1_pyramid.push_back(kp_top);
		kp2_pyramid.push_back(kp_top);
	}

	//开始迭代了

	for (int i = pyramid_size - 1; i >= 0; i--)
	{
		success.clear();
		OpticalFlowSingleLevel(img1_pyramid[i], img2_pyramid[i], kp1_pyramid, kp2_pyramid, success, inverse, true);
	

		if (i > 0)
		{
			for (auto &kp : kp1_pyramid) {
				kp.pt /= scal;
			}
			for (auto &kp : kp2_pyramid) {
				kp.pt /= scal;
			}
		}
	}
	//迭代完毕后的kp2是最需要的
	for (auto &kp_end : kp2_pyramid) { kp2.push_back(kp_end); };
}

注意：
1.坐标的修正
if (i > 0)
{
for (auto &kp : kp1_pyramid) {
kp.pt /= scal;
}
for (auto &kp : kp2_pyramid) {
kp.pt /= scal;
}

  这里是利用参数的引用间接地修改了关键点的坐标，其实kp1_pyramid是没有变化的，只是对原坐标缩放，关键是kp2_pyramid，kp2_pyramid是求解得到的特征点。

  从顶层到底层，与kp2_pyramidkp1_pyramid的匹配程度越来越高，在每层求解后均需要进行缩放，以便于下一层使用。

2.求解结果的传递

OpticalFlowSingleLevel(img1_pyramid[i], img2_pyramid[i], kp1_pyramid, kp2_pyramid, success, inverse, true);

  认真理解 true 这个参数，我们翻到单层求解的算法中，true对应的参数是has_initial，相应的功能是改变dx,dy的初始化方式，前面说过，LK光流法的关键是求解dx,dy。而多层LK光流是步骤得到好的dx,dy初始值。

当has_initial=true时：
  dx = kp2[i].pt.x - kp.pt.x;
  dy = kp2[i].pt.y - kp.pt.y;

  kp2是上一层求解的结果，经过坐标缩放后，用于初始化dx,dy，这也说明了上层求解得到的kp2是如何在下一层被利用的。

当has_initial=false时：
  dx=0,dy=0。此时为单层求解

  因此，金字塔的核心是关于被优化变量的初始化方式。思想是：每层初始化越接近真实值越好。

参考文献：
[1]《视觉SLAM十四讲》高翔