【SLAM】特征匹配的光流实践

1.前言

在前面所介绍的特征匹配的运算上，通过已经完成的特征匹配进行相机运动位姿估计的计算，这种方法看起来似乎非常可行，但是，他也存在很多问题：

1.特征点的计算是一个非常耗时的过程
2.特征点的数量有限
3.不是所有关键点都是被认为是特征点

这么多问题要怎么去解决呢，其实就是可以用到直接法了。

在讲解直接法之前，我来先谈谈光流法。由于特征点的匹配存在这么多问题，为了简化他，我们可以只提取特征点，不进行匹配，就是不计算描述子，这样就会大大减少他的计算量，但是，不进行特征匹配，如何得到可以利用的后续计算信息呢？

就是使用光流法来预测跟踪特征点。

2.光流

什么是光流？

光流法就是描述图像的像素的运动、移动。

> 光流(Optical flow or optic flow)是关于视域中的物体运动检测中的概念。用来描述相对于观察者的运动所造成的观测目标、表面或边缘的运动。光流法在样型识别、计算机视觉以及其他影像处理领域中非常有用，可用于运动检测、物件切割、碰撞时间与物体膨胀的计算、运动补偿编码，或者通过物体表面与边缘进行立体的测量等等。

一些求光流的算法：

1. 相位相关
2. 块相关(误差绝对值和, 标准化互相关)
3. 梯度约束-相关的对齐
4. 卢卡斯-卡纳德方法（Lucas-Kanade Method）
5. 霍恩·山克方法（Horn Schunck Method）

3.卢卡斯-卡纳德方法（Lucas-Kanade Method）

直接给出官方的定义：

> 在计算机视觉中，卢卡斯-金出方法是一种广泛使用的光流估计的差分方法，这个方法是由Bruce D. Lucas和Takeo Kanade发明的。它假设光流在像素点的邻域是一个常数，然后使用最小平方法对邻域中的所有像素点求解基本的光流方程。

对于一个图像，我们用灰度图去描述它，对于它来说，这个像素点的灰度，在随着时间变化的过程中，是有一个假设：

时间推移，灰度不变假设。

通常情况下，我们可以假设一个领域窗口内，所有像素的运动情况一致，则通过最小二乘法就可以算出图像间的运动速度。

4.Opencv相关API

在框架下的演示代码：

void detectFeature(Mat & gray);
void detectOpticalFlow(Mat& prev , Mat& gray);
int main()
{
	VideoCapture cap;
	cap.open("F:\\visual studio\\Image\\123.mp4");
	Mat gray;
	Mat prev_gray;
	namedWindow("Frame", WINDOW_AUTOSIZE);
	createTrackbar("最大角点数", "Frame", &maxCornerNum, maxTrackbarNum,0,0);
	while (1)
	{
		cap >> frame;	
		cvtColor(frame, gray, COLOR_BGR2GRAY);
		if (prev_gray.empty())
			gray.copyTo(prev_gray);
		
		detectFeature(gray);	
		detectOpticalFlow(prev_gray,gray);
		gray.copyTo(prev_gray);
		
		imshow("Frame", frame);
		while (waitKey(1) == 27)
			break;
	}
 
}
 
void detectFeature(Mat & gray)
{
	double qualityLevel = 0.01;
	double minDistance = 10;
	int blocksize = 3;
	goodFeaturesToTrack(gray, corners[0], maxCornerNum, qualityLevel, minDistance, Mat(), 3);
}
 
void detectOpticalFlow(Mat& prev,Mat& gray)
{
	calcOpticalFlowPyrLK(prev, gray, corners[0], corners[1], status, err, Size(21, 21), 3);
	int k = 0;
	for (int i = 0; i < corners[1].size(); i++)
	{
		double dist = abs(corners[1][i].x - corners[0][i].x) + abs(corners[1][i].y - corners[0][i].y);
		if (dist>2 && status[i])
		{
			corners[1][k++] = corners[1][i];
			circle(frame, corners[0][i], 1, Scalar(255, 0, 0), FILLED, LINE_AA, 0);
			circle(frame, corners[1][i], 2, Scalar(0, 0, 255), FILLED, LINE_AA, 0);
			line(frame, corners[0][i], corners[1][i], Scalar(0, 255, 0), 1, LINE_AA, 0);
		}
	}	

结果如下：

5.卢卡斯-卡纳德方法实践

我们这里使用卢卡斯-卡纳德方法进行编程实践，依然依托opencv这个框架下去完成。这里使用的为角点，不进行匹配。

int main(int argc, char **argv) {

    // images, note they are CV_8UC1, not CV_8UC3
    Mat img1 = imread(file_1, 0);
    Mat img2 = imread(file_2, 0);

    // key points, using GFTT here.
    vector<keypoint> kp1;
    Ptr<gfttdetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector-&gt;detect(img1, kp1);

    // now lets track these key points in the second image
    // first use single level LK in the validation picture
    vector<keypoint> kp2_single;
    vector<bool> success_single;
    OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);

    // then test multi-level LK
    vector<keypoint> kp2_multi;
    vector<bool> success_multi;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi, true);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>&gt;(t2 - t1);
    cout &lt;&lt; "optical flow by gauss-newton: " &lt;&lt; time_used.count() &lt;&lt; endl;

    // use opencv's flow for validation
    vector<point2f> pt1, pt2;
    for (auto &amp;kp: kp1) pt1.push_back(kp.pt);
    vector<uchar> status;
    vector<float> error;
    t1 = chrono::steady_clock::now();
    cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>&gt;(t2 - t1);
    cout &lt;&lt; "optical flow by opencv: " &lt;&lt; time_used.count() &lt;&lt; endl;

    // plot the differences of those functions
    Mat img2_single;
    cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
    for (int i = 0; i &lt; kp2_single.size(); i++) {
        if (success_single[i]) {
            cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat img2_multi;
    cv::cvtColor(img2, img2_multi, CV_GRAY2BGR);
    for (int i = 0; i &lt; kp2_multi.size(); i++) {
        if (success_multi[i]) {
            cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat img2_CV;
    cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);
    for (int i = 0; i &lt; pt2.size(); i++) {
        if (status[i]) {
            cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
        }
    }

    cv::imshow("tracked single level", img2_single);
    cv::imshow("tracked multi level", img2_multi);
    cv::imshow("tracked by opencv", img2_CV);
    cv::waitKey(0);

    return 0;
}

6.多层光流

一般情况下，金字塔特征跟踪算法描述如下:首先，在图像的顶层计算光流和仿射变换矩阵，将上一层的计算结果作为初值传递给下一层的图像。基于这个初值，该层的图像计算该层的光流和仿射变换矩阵。然后将这一层的光流和仿射矩阵作为初值传递到下一层的图像，直到传递到最后一层，即原始图像层。将计算得到的该层的光流和仿射变换矩阵作为最终的光流和仿射变换矩阵的结果。

7.算法流程

8.总结

通过光流的计算，我们可以得到特征点的相关性，通过最小二乘估计得到特征点的速度，但是，这种光流法也是有诸多限制，比如，不能光照突然变化太大，这会导致假设不成立，对于图像采集的密集、连续也有要求。