视觉SLAM实践入门——（18）视觉里程计之单层光流法

单层（稀疏）光流法的过程

1、寻找GFTT角点

2、对于每个角点、每次迭代，使用8x8窗口计算：

    （1）求误差

    （2）求雅可比（源码中多处添加负号，不直观，下面附的代码已经修改为书上公式的直观表达）

    （3）求H、b

    （4）求解增量方程，更新优化变量，重复循环

其中，源码中并没有直接使用某点的像素深度，而是使用2x2窗口进行局部平滑处理后的像素深度。对于opencv的光流法，好像是先对图像进行全局平滑处理，再取像素深度

另外，源码的单层光流法并没有假定8x8窗口的像素都具有相同运动，对于下一节的多层光流法却是假定了相同运动

 

注意事项

 编译本节代码需要使用 opencv4，安装和使用的注意事项参考 https://blog.youkuaiyun.com/floatinglong/article/details/117173740

 

对于下面这种找不到 CV_GRAY2BGR 的错误，原因在于新版的 opencv 修改了它的名字

error: ‘COLOR_GRAY2BGR’ was not declared in this scope

将代码中的 CV_GRAY2BGR 改为 COLOR_GRAY2BGR 即可

 

对于找不到 COLOR_GRAY2BGR 的错误，原因可能在于没有引用作用域，在代码头部添加

using namespace cv;

 

直接运行代码可能会出现错误，提示没有安装 libgtk2.0-dev and pkg-config，使用apt安装，重新对opencv进行配置编译安装即可（删除CMakeCache.txt，重新编译不会花太长时间，因为只是更改某些配置项，不需要全部重新编译）

sudo apt install libgtk2.0-dev pkg-config

 

 

#include <iostream>
#include <string>
using namespace std;

//#include <list>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
using namespace cv;

#include <Eigen/Core>
#include <Eigen/Dense>


//定义一个用于光流跟踪的类
class OpticalFlowTracker
{
public:
	OpticalFlowTracker(
		const Mat &img1,
		const Mat &img2,
		const vector<KeyPoint> &kp1,
		vector<KeyPoint> &kp2,
		vector<bool> &success,
	) : _img1(img1), _img2(img2), _kp1(kp1), _kp2(kp2), _success(success){};

	void calculateOpticalFlow(const Range &range);
		 
private:
	const Mat &_img1;
	const Mat &_img2;
	const vector<KeyPoint> &_kp1;
	vector<KeyPoint> &_kp2;
	vector<bool> &_success;
};


//下面使用了源码，没有修改
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    //不严谨的边界检测
	//1：后面使用了2x2窗口进行平滑处理，如果传入点在最后一行会导致指针越界
	//2：如果传入点在最后一列，不再是使用2*2方块进行平滑处理（其中一个点位于下一行的第一列）
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
																			
    unsigned char *data = &img.data[int(y) * img.step + int(x)];		
	
    float xx = x - floor(x);
    float yy = y - floor(y);
	
	//平滑处理
    return 
	float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

void OpticalFlowTracker::calculateOpticalFlow(const Range &range)
{
	//8x8窗口
	int half_patch_size = 4;
	int iterations = 10;

	for(size_t i = range.start; i < range.end; i++)
	{
		auto kp = _kp1[i];
		double dx = 0, dy = 0;
		//多层光流，通过上/下层光流知道kp2[i].pt
		if(_has_initial)
		{
			dx = _kp2[i].pt.x - kp.pt.x;
			dy = _kp2[i].pt.y - kp.pt.y;
		}

		double cost = 0, lastCost = 0;
		bool success = true;

		Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
		Eigen::Vector2d b = Eigen::Vector2d::Zero();
		Eigen::Vector2d J;
		for(int iter = 0; iter < iterations; iter++)
		{
            H = Eigen::Matrix2d::Zero();
            b = Eigen::Vector2d::Zero();
		
			cost = 0;
			//对于每个点，使用8x8窗口(-4~+3)求增量方程
			for(int x = -half_patch_size; x < half_patch_size; x++)
				for(int y = -half_patch_size; y < half_patch_size; y++)
				{
					double err = GetPixelValue(_img2, kp.pt.x + x + dx, kp.pt.y + y + dy) - 
								GetPixelValue(_img1, kp.pt.x + x, kp.pt.y + y);

					//求雅可比
                    J = 1.0 * Eigen::Vector2d(
					    //dI/dx=[(x3-x2)+(x2-x1)] / 2
                        0.5 * (GetPixelValue(_img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
                               GetPixelValue(_img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
                        0.5 * (GetPixelValue(_img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
                               GetPixelValue(_img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
				    );			
					
					//求H、b
					cost += err*err;
					b += err*J;
					H += J*J.transpose();
				}
			//解增量方程
			Eigen::Vector2d update = H.ldlt().solve(b);

			if(isnan(update[0]))
			{
				cout << (int)i << "\t:isnan" << endl;
				success = false;			
				break;
			}

			if(iter > 0 && cost > lastCost)			
				break;			
				
			dx += update[0];
			dy += update[1];
			lastCost = cost;
			success = true;

			if(update.norm() < 1e-2)		
				break;																				
		}		
		
		_success[i] = success;
		_kp2[i].pt = kp.pt + Point2f(dx, dy);
	}

}

void OpticalFlowSingleLevel(const Mat &img1, const Mat &img2,
	const vector<KeyPoint> &kp1, vector<KeyPoint> &kp2, vector<bool> &success)
{
	kp2.resize(kp1.size());
	success.resize(kp1.size());
	OpticalFlowTracker tracker(img1, img2, kp1, kp2, success);
	//并行计算函数，参数：循环列表，函数调用（这里使用了绑定，参数：函数调用，对象，绑定参数列表（不需要绑定的使用占位符_1、_2、…））
	//opencv3.2.0好像是不支持给parallel_for_传入bind，需要升级opencv4的原因可能在这里
	//测试发现，源码中使用并行运算会出现一些随机错误，推测是多线程之间数据共享的原因
	//parallel_for_(Range(0, kp1.size()),
	//				std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
		
	//不使用并行计算
	tracker.calculateOpticalFlow(Range(0, kp1.size()));
}

int main(int argc, char **argv)
{
	if(argc != 3)
	{
		cout << "err" << endl;
		return -1;
	}
	
	//读取灰度图
	Mat img1 = imread(argv[1], IMREAD_GRAYSCALE);
	Mat img2 = imread(argv[2], IMREAD_GRAYSCALE);
	
	//检测GFTT角点，最多500个，最小特征值0.01，最小距离20
	vector<KeyPoint> kp1;
	Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20);
	detector->detect(img1, kp1);

	//使用单层光流法跟踪img1的特征点在img2的位置，success_single标记特征点是否跟丢
	vector<KeyPoint> kp2_single;
	vector<bool> success_single;
	OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);
	
/*
	//跟踪前后坐标信息
	for(int i = 0; i < (int)kp1.size(); i++)
	{
		Mat p1 = (Mat_<double>(1, 2) << kp1[i].pt.x, kp1[i].pt.y);		
		
		cout << i << "\t:" << p1 << endl;
	}	
	//跟丢的点被标记为 0
	//从这里可以看出，源码使用并行计算会随机跟丢一些点，opencv库的光流法并不会有这种问题，将源码程序改为不使用并行计算，也不会跟丢
	for(int i = 0; i < (int)kp1.size(); i++)
		cout << i << "\t:" << success_single[i] << endl;
*/
	
	//画点
	Mat img_single;
	cv::cvtColor(img2, img_single, COLOR_GRAY2BGR);
	for(int i = 0; i < (int)kp1.size(); i++)
	{
		if(success_single[i])
		{
            cv::circle(img_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
			//从kp1指向kp2
            cv::line(img_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));		
		}
	}
	
	imshow("single optical flow", img_single);

	waitKey(0);

	return 0;
}