视觉里程计02 基于特征匹配的位姿估计

概述

• 特征点的投影模型为 (p=frac{1}{Z} KP)，(P)为世界坐标系下某点的坐标（(Z)为z方向的坐标），(p)为对应图像特征点。(K)为内参，在标定好的相机下，(K)已知
• 根据对极几何约束，假设(p_{2})为相机位姿运动(R)，(t)后与前一帧的特征点(p_{1})匹配的特征点，则有

[s_1p_1 = KP ]

[s_2p_2 = K(RP+t) ]

• 参考视觉slam14讲的推导，这里可以得到对极约束

[{p}_2^T{K^{ - T}}{t^ wedge }RK{H^{ - 1}}{p_1} = 0 ]

可以通过8点法求解本质矩阵进而得到(R)，(t)

• 每两帧之间的位姿递推误差积累很快，因此直接递推的位姿是不太稳定的。
• (t)的缩放尺寸不确定，因此不能获得绝对位置

测试代码

主要基于视觉slam14讲的代码，稍微改动的测试，尽管能够求解姿态但是并不十分准确，后续考虑使用双目相机实现定位功能

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include <iostream>
#include "opencv2/features2d/features2d.hpp"
#include <vector>
#include <time.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <Windows.h>
//#include "stdafx.h"

using namespace cv;
using namespace std;

void find_feature_matches(
	const Mat& img_1, const Mat& img_2,
	std::vector<KeyPoint>& keypoints_1,
	std::vector<KeyPoint>& keypoints_2,
	std::vector< DMatch >& matches);

void pose_estimation_2d2d(
	std::vector<KeyPoint> keypoints_1,
	std::vector<KeyPoint> keypoints_2,
	std::vector< DMatch > matches,
	Mat& R, Mat& t);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d& p, const Mat& K);


int main()
{
	VideoCapture cap1;
	//VideoCapture cap2;
	cap1.open(1);//白色摄像头
	//cap2.open(2);//黑色摄像头
	//if (!cap1.isOpened()||!cap2.isOpened())
	if (!cap1.isOpened())
	{
		return -1;
	}
	//将摄像头从640*480改成320*240，速度从200ms提升至50ms
	//cap1.set(CV_CAP_PROP_FRAME_WIDTH, 320);
	//cap1.set(CV_CAP_PROP_FRAME_HEIGHT, 240);
	
	//cap2.set(CV_CAP_PROP_FRAME_WIDTH, 320);
	//cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 240);
	//namedWindow("Video", 1);
	//namedWindow("Video", 2);
	//namedWindow("pts", 3);
	//Mat frame;
	
	Mat img_1;
	Mat img_2;
	while (1)
	{
		cap1 >> img_1;
		Sleep(10);
		cap1 >> img_2;
		if (!img_1.data || !img_2.data)
		{
			cout << "error reading images " << endl;
			return -1;
		}
		vector<KeyPoint> keypoints_1, keypoints_2;
		vector<DMatch> matches;
		find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
		//cout << "一共找到了" << matches.size() << "组匹配点" << endl;

		//-- 估计两张图像间运动
		Mat R, t;
		pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
		//cout << "R:" << endl << R << endl;
		//cout << "t:" << endl << t << endl;
		////-- 验证E=t^R*scale
		//Mat t_x = (Mat_<double>(3, 3) <<
		//	0, -t.at<double>(2, 0), t.at<double>(1, 0),
		//	t.at<double>(2, 0), 0, -t.at<double>(0, 0),
		//	-t.at<double>(1.0), t.at<double>(0, 0), 0);

		//cout << "t^R=" << endl << t_x*R << endl;

		////-- 验证对极约束
		//Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
		//for (DMatch m : matches)
		//{
		//	Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
		//	Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);
		//	Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
		//	Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
		//	Mat d = y2.t() * t_x * R * y1;
		//	cout << "epipolar constraint = " << d << endl;
		//}
		waitKey(1);
	}
	cap1.release();
	//cap2.release();
	return 0;
}

void find_feature_matches(const Mat& img_1, const Mat& img_2,
	std::vector<KeyPoint>& keypoints_1,
	std::vector<KeyPoint>& keypoints_2,
	std::vector< DMatch >& matches)
{
	//-- 初始化
	Mat descriptors_1, descriptors_2;
	// used in OpenCV3 
	Ptr<FeatureDetector> detector = ORB::create();
	Ptr<DescriptorExtractor> descriptor = ORB::create();
	// use this if you are in OpenCV2 
	// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
	// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	//-- 第一步:检测 Oriented FAST 角点位置
	detector->detect(img_1, keypoints_1);
	detector->detect(img_2, keypoints_2);

	//-- 第二步:根据角点位置计算 BRIEF 描述子
	descriptor->compute(img_1, keypoints_1, descriptors_1);
	descriptor->compute(img_2, keypoints_2, descriptors_2);

	//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
	vector<DMatch> match;
	//BFMatcher matcher ( NORM_HAMMING );
	matcher->match(descriptors_1, descriptors_2, match);

	//-- 第四步:匹配点对筛选
	double min_dist = match[0].distance, max_dist = match[0].distance;

	//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
	for (int i = 0; i < descriptors_1.rows; i++)
	{
		double dist = match[i].distance;
		if (dist < min_dist) min_dist = dist;
		if (dist > max_dist) max_dist = dist;
	}

	//printf("-- Max dist : %f 
", max_dist);
	//printf("-- Min dist : %f 
", min_dist);

	//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
	for (int i = 0; i < descriptors_1.rows; i++)
	{
		if (match[i].distance <= max(2 * min_dist, 30.0))
		{
			matches.push_back(match[i]);
		}
	}
}


Point2d pixel2cam(const Point2d& p, const Mat& K)
{
	return Point2d
	(
		(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
		(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
	);
}


void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
	std::vector<KeyPoint> keypoints_2,
	std::vector< DMatch > matches,
	Mat& R, Mat& t)
{
	// 相机内参,需要标定得到
	/*1225.22831056496	36.6177252813478	342.784169613124
		0	1178.20016318321	187.290755011276
		0	0	1*/
	/*1296.76842892674	46.6256354215592	409.717933143672
		0	1210.08953016730	69.8389243159229
		0	0	1*/
	//Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	Mat K = (Mat_<double>(3, 3) << 1296.76842892674, 46.6256354215592, 409.717933143672, 0, 1210.08953016730, 69.8389243159229, 0, 0, 1);

	//-- 把匹配点转换为vector<Point2f>的形式
	vector<Point2f> points1;
	vector<Point2f> points2;

	for (int i = 0; i < (int)matches.size(); i++)
	{
		points1.push_back(keypoints_1[matches[i].queryIdx].pt);
		points2.push_back(keypoints_2[matches[i].trainIdx].pt);
	}

	//-- 计算基础矩阵
	Mat fundamental_matrix;
	fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);
	//cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;

	//-- 计算本质矩阵
	Point2d principal_point(409.717933143672, 69.8389243159229);	//相机光心, 标定值
	double focal_length = 1296.76842892674;			//相机焦距, 标定值
	Mat essential_matrix;
	essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
	//cout << "essential_matrix is " << endl << essential_matrix << endl;

	//-- 计算单应矩阵
	Mat homography_matrix;
	homography_matrix = findHomography(points1, points2, RANSAC, 3);
	//cout << "homography_matrix is " << endl << homography_matrix << endl;

	//-- 从本质矩阵中恢复旋转和平移信息.
	recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
	//cout << "R is " << endl << R << endl;
	//cout << "t is " << endl << t << endl;
	cout << R << endl;
}