视觉SLAM实践入门——（10）特征提取和匹配（修复源码中的段错误bug）

最新推荐文章于 2022-10-08 19:29:06 发布

floatinglong

最新推荐文章于 2022-10-08 19:29:06 发布

阅读量1.2k

点赞数 6

分类专栏： SLAM 文章标签： slam

本文链接：https://blog.csdn.net/floatinglong/article/details/116279765

版权

SLAM 专栏收录该内容

30 篇文章 22 订阅

订阅专栏

这一节的第一部分使用opencv提取关键点、计算描述子、匹配特征点

第二部分则根据前面的原理，写一个简单的计算描述子、匹配特征点的算法（都是SLAM十四讲的源码，第二部分源码中有段错误，不能直接运行，需要修改），经过比较发现，使用opencv的算法效率较低

第一部分和第二部分中算法运行所用时间如下

第一部分—使用opencv

opencv库封装了与特征提取和匹配相关的函数，程序的运行思路如下

1、以RGB格式读取图像

2、检测ORB角点

3、根据角点附近的像素计算角点的描述子

4、通过比较汉明距离，匹配特征点

5、去除误匹配的点数（依据：汉明距离大于最小距离的两倍属于误匹配）

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <chrono>

using namespace cv;
using namespace std;

int main(int argc, char **argv)
{
	if(argc != 3)
	{
		cout << "input img1 ,img2" << endl;
		return -1;
	}
	
	//读取图像
	Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);	//以RGB格式读取
	Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
	assert(img1.data != nullptr && img2.data != nullptr);	//判断是否为图片

	//初始化
	std:vector<KeyPoint> kp1, kp2;	//关键点
	Mat desc1, desc2;				//描述子
	Ptr<FeatureDetector> detector = ORB::create();	//用于计算角点
	Ptr<DescriptorExtractor> desc = ORB::create();	//用于计算描述子
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");	//用于匹配描述子（利用汉明距离）
	
	//检测ORB角点
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	detector->detect(img1, kp1);
	detector->detect(img2, kp2);

	//根据角点计算BRIEF描述子
	desc->compute(img1, kp1, desc1);
	desc->compute(img2, kp2, desc2);
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
	cout << "extract ORB cost = " << time_used.count() << "s" << endl;
	
	//显示特征点
	Mat outimg1;
	drawKeypoints(img1, kp1, outimg1, Scalar::all(-1),DrawMatchesFlags::DEFAULT);
	imshow("ORB features", outimg1);
	
	//使用汉明距离，对两张图像的描述子进行匹配
	vector<DMatch> matches;
	t1 = chrono::steady_clock::now();
	matcher->match(desc1, desc2, matches);
	t2 = chrono::steady_clock::now();
	time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
	cout << "match ORB cost = " << time_used.count() << "s" << endl;
	
	//筛选匹配点对
	auto min_max = minmax_element(matches.begin(), matches.end(),
		[](const DMatch &m1, const DMatch &m2){return m1.distance < m2.distance;});
	
	double min_dist = min_max.first->distance;
	double max_dist = min_max.second->distance;
	
	printf("Max_dist = %f\n", max_dist);
	printf("Min_dist = %f\n", min_dist);
	
	//如果描述子之间的距离大于两倍最小距离，认为匹配有误；有时最小距离非常小，因此设置经验值30为下限
	std::vector<DMatch> good_matches;
	for(int i = 0; i < desc1.rows; i++)
		if(matches[i].distance <= max(2*min_dist, 30.0)) good_matches.push_back(matches[i]);
		
	//绘制匹配点对
	Mat img_match;
	Mat img_goodmatch;
	drawMatches(img1, kp1, img2, kp2, matches, img_match);	
	drawMatches(img1, kp1, img2, kp2, good_matches, img_goodmatch);
	imshow("all matches", img_match);
	imshow("good matches", img_goodmatch);
	
	waitKey(0);
		
	return 0;
	
}

第二部分—根据原理写一个简单的计算描述子、匹配特征点的算法

在代码中，依旧使用opencv提取FAST关键点，计算描述子、匹配特征点的原理与前面章节所述相同

需要注意的是，源码ComputeORB函数中对出界特征点的筛选有瑕疵，修改后正确代码应该是下面这样的（看了好几篇文章，好像都是把源码CV到自己的博客，加上注释，却没有修改代码错误的地方，属实是坑。。。）

if(kp.pt.x <= half_boundary || 
	kp.pt.y <= half_boundary || 
	kp.pt.x >= img.cols - half_boundary || 
	kp.pt.y >= img.rows - half_boundary)	//出界

为了简洁，源码的 ORB_pattern 表被单独移动到 ORB_pattern.cpp 中

_mm_popcnt_u32函数用于计算32位变量中1的位数，为了使用这个函数，需要包含头文件nmmintrin.h，并且在CMakeLists.txt中添加下面一行，以增加一个编译选项，支持SSE指令集（当然我们也可以不用这个函数，自己计算）

set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")

其他的说明在代码中已经注释了，不再赘述

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

#include <nmmintrin.h>

#include "ORB_pattern.cpp"

using namespace std;
using namespace cv;

typedef vector<uint32_t> DescType;

void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints, vector<DescType> &descriptors)
{
	const int half_patch_size = 8;	//图像块的半长
	const int half_boundary = 16;	//图像块16*16=256
	int bad_points = 0;		//出界的点数
	for(auto &kp: keypoints)	
	{
		if(kp.pt.x <= half_boundary || 
			kp.pt.y <= half_boundary || 
			kp.pt.x >= img.cols - half_boundary || 
			kp.pt.y >= img.rows - half_boundary)	//出界
		{
			bad_points++;
			descriptors.push_back({});
			continue;
		}

		//如果没有出界，计算一阶矩
		float m01 = 0, m10 = 0;	
		for(int dx = -half_patch_size; dx < half_patch_size; ++dx)
			for(int dy = -half_patch_size; dy < half_patch_size ; ++dy)
			{
				unsigned char pixel = img.at<unsigned char>(kp.pt.y + dy, kp.pt.x + dx);
				m10 += dx * pixel;
				m01 += dy * pixel;
			}
		//计算特征点方向
		float m_sqrt = sqrt(m01 * m01 + m10 * m10) + 1e-18;
		float sin_theta = m01 / m_sqrt;
		float cos_theta = m10 / m_sqrt;

		DescType desc(8, 0);	//8个描述子向量，每个向量中的元素占据32位，初始化为0；每个描述子使用256位二进制数进行描述
		for(int i = 0; i < 8; i++)	//处理每个向量
		{
			uint32_t d = 0;
			for(int k = 0; k < 32; k++)	//处理每一位
			{
				//原理参考https://www.cnblogs.com/alexme/p/11345701.html
				//旋转公式参考https://blog.csdn.net/sss_369/article/details/90182696
				//ORB_pattern含义是在16*16图像块中按高斯分布选取点对，选出来的p、q是未经过旋转的
				//pp和qq利用了特征点的方向，计算了旋转的位置，体现了ORB的旋转不变性
				//img.at(y,x)，参数是点所在的行列而不是点的坐标	
				int idx_pq = i*8 + k;
				cv::Point2f p(ORB_pattern[idx_pq*4], ORB_pattern[idx_pq*4 + 1]);
				cv::Point2f q(ORB_pattern[idx_pq*4 + 2], ORB_pattern[idx_pq*4 + 3]);

				cv::Point2f pp = cv::Point2f(cos_theta * p.x - sin_theta * p.y, sin_theta * p.x + cos_theta * p.y) + kp.pt;
				cv::Point2f qq = cv::Point2f(cos_theta * q.x - sin_theta * q.y, sin_theta * q.x + cos_theta * q.y) + kp.pt;
				if(img.at<unsigned char>(pp.y, pp.x) < img.at<unsigned char>(qq.y, qq.x))
					d |= 1<<k;		
			}
			desc[i] = d;
		}
		descriptors.push_back(desc);
	}
	cout << "bad_point and total_point:" << bad_points << " " << keypoints.size() << endl;
}

void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches)
{
	const int d_max = 40;	//匹配依据：汉明距离小于40认为点对匹配	

	//暴力匹配—对img1中的所有关键点与img2中所有关键点进行匹配
	//汉明距离—两个二进制描述子，不同位数的个数，例如1100和0101，汉明距离是2
	//距离小于一定的值，认为两个特征值是匹配的
	for(size_t i1 = 0; i1 < desc1.size(); ++i1)
	{
		if(desc1[i1].empty())	continue;	//关键点出界，描述子为空
		cv::DMatch m{(int)i1, 0, 256};	//匹配点对的默认参数
		for(size_t i2 = 0; i2 < desc2.size(); ++i2)
		{
			if(desc2[i2].empty()) continue;

			int distance = 0;
			for(int k = 0; k < 8; k ++)	distance += _mm_popcnt_u32(desc1[i1][k] ^ desc2[i2][k]);	//_mm_popcnt_u32返回u32变量中含1的位数
			
			if(distance < d_max && distance < m.distance)	//符合匹配条件，设置匹配点对，参数1是img1的关键点下标（第一层for循环中设置），参数2是img2的关键点下标，参数3是点对距离
			{
				m.distance = distance;
				m.trainIdx = i2;
			}//如果不符合匹配条件，使用默认参数
		}
		if(m.distance < d_max)	matches.push_back(m);
	}
}

int main(int argc, char **argv)
{
	if(argc != 3)
	{
		cout << "input 3 image!" << endl;
		return -1;
	}

	//读取图像
	Mat img1 = imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
	Mat img2 = imread(argv[2], CV_LOAD_IMAGE_GRAYSCALE);
	assert(img1.data != nullptr && img2.data != nullptr);
	//检测角点
	vector<KeyPoint> kp1, kp2;
	FAST(img1, kp1, 40);
	FAST(img2, kp2, 40);
	//计算描述子
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

	vector<DescType> desc1, desc2;
	ComputeORB(img1, kp1, desc1);
	ComputeORB(img2, kp2, desc2);

	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
	cout << "ComputeORB cost = " << time_used.count() << "s" << endl;
	//汉明距离匹配
	t1 = chrono::steady_clock::now();

	vector<DMatch> matches;
	BfMatch(desc1, desc2, matches);

	t2 = chrono::steady_clock::now();
	time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
	cout << "Match cost = " << time_used.count() << "s" << endl;
	//显示，保存图片，outImg是灰度图，outImgColor是彩图
	Mat outImg, outImgColor;


	Mat img3 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
	Mat img4 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

	drawMatches(img1, kp1, img2, kp2, matches, outImg);
	drawMatches(img3, kp1, img4, kp2, matches, outImgColor);

	imshow("matches_img", outImg);
	imshow("matches_img_color", outImgColor);

	imwrite("matches_img.png", outImg);
	imwrite("matches_img_color.png", outImgColor);

	waitKey(0);

	return 0;
}