视觉SLAM实践入门——（11）用对极约束求解相机运动

使用对极约束求解相机运动的步骤：

1、提取ORB特征，匹配点对

2、根据匹配点对计算基础矩阵 E

3、分解 E，得到相机位姿 R，t

使用估计的 R，t 可以验证各个特征点对的对极约束误差（理论上误差精确为0）

经过验证，使用点对计算的 E，与使用R，t恢复的 E，各元素相差一个同一个非0常数，满足本质矩阵的尺度不变性；通过计算 验证 F，发现前后计算的 F 并不相等，原因还没找到

由于 E 本身具有尺度等价性，它分解得到的 R,t 也有一个尺度等价性。而 R ∈ SO(3) 自身具有约束，所以 t 具有一个尺度。因此在分解过程中，对 t 乘以任意非零常数，分解都是成立的，一般将 t 归一化处理，让它的长度等于 1。能够对 t 长度的归一化，直接导致了单目视觉的尺度不确定性

单目初始化不能只有纯旋转，必须要有一定程度的平移。从 E 分解到 R,t 的过程中，如果相机发生的是纯旋转，导致 t 为零，那么，得到的E 也将为零，这将导致无法求解 R。不过，此时可以依靠 H 求取旋转，但仅有旋转时，无法用三角测量估计特征点的空间位置。如果初始化时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。相对的，如果把相机左右移动而不是原地旋转，就容易让单目 SLAM 初始化

当可能存在误匹配的情况时，会倾向于使用随机采样一致性（Random Sample Concensus, RANSAC）来求，而不是最小二乘。RANSAC 是一种通用的做法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据

#include <iostream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace cv;

void OrbFeaturesMatch(const Mat &img1, const Mat &img2,
			vector<KeyPoint> &kp1, vector<KeyPoint> &kp2,
			vector<DMatch> &matches)
{
	Mat desc1, desc2;
	
	Ptr<FeatureDetector> detector = ORB::create();
	Ptr<DescriptorExtractor> desc = ORB::create();
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	
	//1、检测角点
	detector->detect(img1, kp1);
	detector->detect(img2, kp2);
	//2、计算描述子
	desc->compute(img1, kp1, desc1);
	desc->compute(img2, kp2, desc2);
	//3、匹配点对
	vector<DMatch> match;
	matcher->match(desc1, desc2, match);
	//4、检测误匹配
	double minDist = 10000, maxDist = 0;
	for(int i = 0; i < (int)match.size(); i++)
	{
		double dist = match[i].distance;
		
		minDist = minDist > dist ? dist : minDist;
		maxDist = maxDist < dist ? dist : maxDist;
	}
	
	cout << "minDist and maxDist = " << minDist << " and " << maxDist << endl;
	
	double matchDist = max(30.0, minDist * 2);
	for(int i = 0; i < (int)match.size(); i++)
		if(match[i].distance <= matchDist)
			matches.push_back(match[i]);			
}

void estimateMoving(const vector<KeyPoint> &kp1, const vector<KeyPoint> &kp2, const vector<DMatch> &matches, Mat &R, Mat &t)
{
	//取出匹配点对坐标
	vector<Point2f> p1, p2;
	for(int i = 0; i < matches.size(); i++)
	{
		//DMatch中有3个成员，queryIdx，trainIdx，distance
		//queryIdx是特征点第一张图的索引，trainIdx是特征点在第二张图的索引
		p1.push_back(kp1[matches[i].queryIdx].pt);
		p2.push_back(kp2[matches[i].trainIdx].pt);
	}
	
	//1、计算基础矩阵、本质矩阵、单应矩阵
	Mat fundamentalMat = findFundamentalMat(p1, p2, CV_FM_8POINT);	//8点法
	cout << "fundamentalMat :\n" << fundamentalMat << endl;
	
	double focalLength = 520.9;
	Point2d principalPoint(325.1, 249.7);
	Mat essentialMat = findEssentialMat(p1, p2, focalLength, principalPoint);
	cout << "essentialMax :\n" << essentialMat << endl;
	
	//场景不是平面时，单应矩阵意义不大
	Mat homographyMat = findHomography(p1, p2, RANSAC, 3);	//RANSAC：随机采样一致性，最大允许重投影错误阈值是3
	cout << "homographyMat :\n" << homographyMat << endl;

	//2、分解矩阵，求解相机运动R，t
	recoverPose(essentialMat, p1, p2, R, t, focalLength, principalPoint);
	cout << "R :\n" << R << endl;
	cout << "t :" << t.t() << endl;
}

Point2d pixelToNor(const Point2d &p, const Mat &k)
{
	//像素坐标 = 内参矩阵 * 归一化坐标：p = K * x（齐次坐标运算）
	//下式计算的原理：x = fx * p_x + cx，y = fy * p_y + cy（记像素坐标(x, y)，归一化坐标(p_x, p_y)）
	//p_x = (x - cx) / fx，p_y = (y - cy) / fy
	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));
}

int main(int argc, char **argv)
{
	if(argc != 3)
	{
		cout << "input 2 images" << endl;
		return -1;
	}
	
	Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
	Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

	//1、匹配特征点
	vector<KeyPoint> kp1, kp2;
	vector<DMatch> matches;
	OrbFeaturesMatch(img1, img2, kp1, kp2, matches);
	
	//2、估计相机运动
	Mat R, t;
	estimateMoving(kp1, kp2, matches, R, t);

	//3、验证本质矩阵的尺度不变性
	Mat tx = (	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						);	//t映射的反对称矩阵
	Mat E(tx*R);
	cout << "E : \n" << E << endl;	//tx*R 与前面输出的 E ，各元素之间相差一个非零常数

	//4、验证对极约束	
	Mat k = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	for(int i = 0; i < (int)matches.size(); i++)
	{
		//取匹配点对的像素坐标
		Point2d p1(kp1[matches[i].queryIdx].pt.x, kp1[matches[i].queryIdx].pt.y);
		Point2d p2(kp2[matches[i].trainIdx].pt.x, kp2[matches[i].trainIdx].pt.y);
		
		//计算归一化坐标的(x, y)
		Point2d y1 = pixelToNor(p1, k);
		Point2d y2 = pixelToNor(p2, k);
		
		//构造归一化坐标
		Mat x1 = (Mat_<double>(3, 1) << y1.x, y1.y, 1);
		Mat x2 = (Mat_<double>(3, 1) << y2.x, y2.y, 1);
		
		printf("对第%03d个特征点的对极约束误差为：", i);
		cout << x2.t() * E * x1 << endl;
	}
	
	return 0;
}