本征矩阵恢复相机姿态（码—基于opencv）

理论知识参考于基于图像的三维重建——对极几何

在这里用的opencv是440版本的，接下来就是相应的代码，基于八点法来求取基础矩阵。

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

using namespace std;
using namespace cv;

struct Correspondence2D2D
{
	double p1[2];
	double p2[2];
};

typedef std::vector<Correspondence2D2D> Correspondences2D2D;

Mat FundamentalMatrix(3, 3, CV_64FC1);
Mat EssentialMatrix(3, 3, CV_64FC1);

//用于测试相机姿态的正确性
Vec2d p1 = { 0.18012331426143646, -0.15658402442932129 };
Vec2d p2 = { 0.2082643061876297, -0.035404585301876068 };

//第一个相机的内外参数
double f1= 0.972222208;

//第二个相机的内外参数
double f2= 0.972222208;

Mat hstack(Mat const & K, Mat const & t)
{
	int rows = K.rows;
	int cols = K.cols+t.cols;
	Mat P(rows, cols, CV_64FC1);
	for (int i = 0; i < K.rows; i++)
		for (int j = 0; j < K.cols; j++)
			P.at<double>(i, j) = K.at<double>(i, j);
	for (int i = 0; i < t.rows; i++)
		for (int j = 0; j < t.cols; j++)
			P.at<double>(i, K.cols + j) = t.at<double>(i, j);
	return P;
}

/**
 * \description 对匹配点进行三角化得到空间三维点
 * @param p1 -- 第一幅图像中的特征点
 * @param p2 -- 第二幅图像中的特征点
 * @param K1 -- 第一幅图像的内参数矩阵
 * @param R1 -- 第一幅图像的旋转矩阵
 * @param t1 -- 第一幅图像的平移向量
 * @param K2 -- 第二幅图像的内参数矩阵
 * @param R2 -- 第二幅图像的旋转矩阵
 * @param t2 -- 第二幅图像的平移向量
 * @return 三维点
 */
Vec3d triangulation(Vec2d const & p1,
	Vec2d const & p2,
	Mat const &K1,
	Mat const &R1,
	Vec3d const & t1,
	Mat const &K2,
	Mat const &R2,
	Vec3d const & t2)
{
	//构造投影矩阵
	Mat P1(3, 4, CV_64FC1);
	Mat P2(3, 4, CV_64FC1);

	Mat KR1 = K1 * R1;
	Mat Kt1 = K1 * t1;
	P1 = hstack(KR1, Kt1);

	Mat KR2 = K2 * R2;
	Mat Kt2 = K2 * t2;
	P2 = hstack(KR2, Kt2);

	Mat A(4, 4, CV_64FC1);
	for (int i = 0; i < 4; i++)
	{
		 //第1个点
		A.at<double>(0, i) = p1[0] * P1.at<double>(2, i) - P1.at<double>(0, i);
		A.at<double>(1, i) = p1[1] * P1.at<double>(2, i) - P1.at<double>(1, i);

		// 第2个点
		A.at<double>(2, i) = p2[0] * P2.at<double>(2, i) - P2.at<double>(0, i);
		A.at<double>(3, i) = p2[1] * P2.at<double>(2, i) - P2.at<double>(1, i);
	}

	Mat bb, cc, V;
	SVD svd;
	svd.compute(A, bb, cc, V, 4);
	V = V.t();
	Vec3d X;
	X[0] = V.at<double>(0, 3) / V.at<double>(3, 3);
	X[1] = V.at<double>(1, 3) / V.at<double>(3, 3);
	X[2] = V.at<double>(2, 3) / V.at<double>(3, 3);

	return X;
}

/**
 * \description 判断相机姿态是否正确，方法是计算三维点在两个相机中的坐标，要求其z坐标大于0，即
 * 三维点同时位于两个相机的前方
 * @param match
 * @param pose1
 * @param pose2
 * @return
 */
bool is_correct_pose(Mat const & R1, Vec3d const &t1, Mat const & R2, Vec3d const &t2)
{
	// 相机内参矩阵
	Mat K1 = Mat::zeros(3, 3, CV_64FC1);
	Mat K2 = Mat::zeros(3, 3, CV_64FC1);
	K1.at<double>(0, 0) = K1.at<double>(1, 1) = f1;
	K2.at<double>(0, 0) = K2.at<double>(1, 1) = f2;
	K1.at<double>(2, 2) = 1.0;
	K2.at<double>(2, 2) = 1.0;
	
	Vec3d p3d = triangulation(p1, p2, K1, R1, t1, K2, R2, t2);

	Mat tt1 = R1 * p3d;
	Mat tt2 = R2 * p3d;
	
	vector<double> x1, x2;
	for (int i = 0; i < 3; i++)
	{
		double xx1 = tt1.at<double>(i);
		double xx2 = t1[i];
		double xx3 = tt2.at<double>(i);
		double xx4 = t2[i];
		x1.push_back(xx1 + xx2);
		x2.push_back(xx3 + xx4);
	}
	return x1[2] > 0.0f && x2[2] > 0.0f;
}

bool calc_cam_poses(Mat F, const double f1, const double f2, Mat &R, Vec3d &t)
{
	// 相机内参矩阵
	Mat K1 = Mat::zeros(3, 3, CV_64FC1);
	Mat K2 = Mat::zeros(3, 3, CV_64FC1);
	K1.at<double>(0, 0) = K1.at<double>(1, 1) = f1;
	K2.at<double>(0, 0) = K2.at<double>(1, 1) = f2;
	K1.at<double>(2, 2) = 1.0;
	K2.at<double>(2, 2) = 1.0;

	EssentialMatrix = K2.t()*F*K1;

	/* 本质矩阵求解的是相机之间的相对姿态，第一个相机姿态可以设置为[I|0], 第二个相机的姿态[R|t]
	 * 可以通过对本质矩阵进行分解来求得, E=U*S*V',其中S是进行尺度归一化之后是diag(1,1,0)
	 */
	Mat W = Mat::zeros(3, 3, CV_64FC1);
	W.at<double>(0, 1) = -1.0;
	W.at<double>(1, 0) = 1.0;
	W.at<double>(2, 2) = 1.0;
	Mat Wt = Mat::zeros(3, 3, CV_64FC1);
	Wt.at<double>(0, 1) = 1.0;
	Wt.at<double>(1, 0) = -1.0;
	Wt.at<double>(2, 2) = 1.0;

	Mat U, S, V;
	SVD svd;
	svd.compute(EssentialMatrix, S, U, V, 4);

	//保证旋转矩阵 det(R) = 1
	if (determinant(U) < 0.0)
	{
		for (int i = 0; i < 3; i++)
			U.at<double>(i, 2) = -U.at<double>(i, 2);
	}
	if (determinant(V) < 0.0)
	{
		for (int i = 0; i < 3; i++)
			V.at<double>(i, 2) = -V.at<double>(i, 2);
	}

	//相机的姿态一共有4种情况
	vector<std::pair< Mat, Vec3d>> poses(4);
	poses[0].first = U * W * V;
	poses[1].first = U * W * V;
	poses[2].first = U * Wt * V;
	poses[3].first = U * Wt * V;
	poses[0].second = U.col(2);
	Mat yy = -U;
	poses[1].second = yy.col(2);
	poses[2].second = U.col(2);
	poses[3].second = yy.col(2);

	// 第一个相机的旋转矩阵R1设置为单位矩阵，平移向量t1设置为0
	Mat R1 = Mat::zeros(3, 3, CV_64FC1);
	R1.at<double>(0, 0) = 1.0;
	R1.at<double>(1, 1) = 1.0;
	R1.at<double>(2, 2) = 1.0;

	Vec3d t1(0.0, 0.0, 0.0);

	bool flags[4];
	for (int i = 0; i < 4; ++i)
	{
		flags[i] = is_correct_pose(R1, t1, poses[i].first, poses[i].second);
	}

	//找到正确的姿态
	if (flags[0] || flags[1] || flags[2] || flags[3])
	{
		for (int i = 0; i < 4; i++)
		{
			if (!flags[i])
				continue;
			R = poses[i].first;
			t = poses[i].second;
		}
		return true;
	}
	return false;
}

int main()
{
	FundamentalMatrix.at<double>(0) = -0.0051918668202215884;
	FundamentalMatrix.at<double>(1) = -0.015460923969578466;
	FundamentalMatrix.at<double>(2) = 0.35260470328319654;
	FundamentalMatrix.at<double>(3) = 0.022451443619913483;
	FundamentalMatrix.at<double>(4) = -0.00079225386526248181;
	FundamentalMatrix.at<double>(5) = -0.027885130552744289;
	FundamentalMatrix.at<double>(6) = -0.35188558059920161;
	FundamentalMatrix.at<double>(7) = 0.032418724757766811;
	FundamentalMatrix.at<double>(8) = -0.005524537443406155;

	
	Mat R;
	Vec3d t;
	if (calc_cam_poses(FundamentalMatrix, f1, f2, R, t))
	{
		cout << "Correct pose found!" << endl;
		cout << "R: " << R << endl;
		cout << "t: " << t << endl;
	}
	
	cout << "Result should be: \n";
	cout << "R: \n"
		 << "0.999827 -0.0119578 0.0142419\n"
		 << "0.0122145 0.999762 -0.0180719\n"
		 << "-0.0140224 0.0182427 0.999735\n";
	cout << "t: \n"
		 << "0.0796625 0.99498 0.0605768\n";
	return 0;
}