视觉SLAM前端——LK光流法

LK光流

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

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

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

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

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

单层光流法

求解的基本流程如下：

特征点的选取
Gauss-Newton法求解Δx,Δy\Delta x,\Delta yΔx,Δ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求解Δx,Δy\Delta x,\Delta yΔx,Δy

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

根据假设2有Cost=∑j=1w∑i=1wI1(u+w,v+w)−I2(u+Δx+w,v+Δy+w)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)Cost=∑j=1w∑i=1wI1(u+w,v+w)−I2(u+Δx+w,v+Δy+w)。

导数JJJ是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,jfor (int iter = 0; iter < iteration; iter++){if (inverse == false) {H = Eigen::Matrix2d::Zero();b = Eigen::Vector2d::Zero();}else {// only reset bb = 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(  //正向梯度，img20.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(  //反向梯度，img10.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 irreversiblecout << "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) {// convergebreak;}}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十四讲》高翔