本征矩阵恢复相机姿态（码—基于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)