基础矩阵F的求法（码—基于Opencv)

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

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

/* 测试8点法求取基础矩阵F
 *
 * [直接线性变换法]
 * 双目视觉中相机之间存在对极约束
 *
 *                       p2'Fp1=0,
 *
 * 其中p1, p2 为来自两个视角的匹配对的归一化坐标，并表示成齐次坐标形式，
 * 即p1=[x1, y1, z1]', p2=[x2, y2, z2],将p1, p2的表达形式带入到
 * 上式中，可以得到如下表达形式
 *
 *          [x2] [f11, f12, f13] [x1, y1, z1]
 *          [y2] [f21, f22, f23]                = 0
 *          [z2] [f31, f32, f33]
 *
 * 进一步可以得到
 * x1*x2*f11 + x2*y1*f12 + x2*f13 + x1*y2*f21 + y1*y2*f22 + y2*f23 + x1*f31 + y1*f32 + f33=0
 *
 * 写成向量形式
 *               [x1*x2, x2*y1,x2, x1*y2, y1*y2, y2, x1, y1, 1]*f = 0,
 * 其中f=[f11, f12, f13, f21, f22, f23, f31, f32, f33]'
 *
 * 由于F无法确定尺度(up to scale, 回想一下三维重建是无法确定场景真实尺度的)，因此F秩为8，
 * 这意味着至少需要8对匹配对才能求的f的解。当刚好有8对点时，称为8点法。当匹配对大于8时需要用最小二乘法进行求解
 *
 *   [x11*x12, x12*y11,x12, x11*y12, y11*y12, y12, x11, y11, 1]
 *   [x21*x22, x22*y21,x22, x21*y22, y21*y22, y22, x21, y21, 1]
 *   [x31*x32, x32*y31,x32, x31*y32, y31*y32, y32, x31, y31, 1]
 * A=[x41*x42, x42*y41,x42, x41*y42, y41*y42, y42, x41, y41, 1]
 *   [x51*x52, x52*y51,x52, x51*y52, y51*y52, y52, x51, y51, 1]
 *   [x61*x62, x62*y61,x62, x61*y62, y61*y62, y62, x61, y61, 1]
 *   [x71*x72, x72*y71,x72, x71*y72, y71*y72, y72, x71, y71, 1]
 *   [x81*x82, x82*y81,x82, x81*y22, y81*y82, y82, x81, y81, 1]
 *
 *现在任务变成了求解线性方程
 *               Af = 0
 *（该方程与min||Af||, subject to ||f||=1 等价)
 *通常的解法是对A进行SVD分解，取最小奇异值对应的奇异向量作为f分解
 *
 *本项目中对矩阵A的svd分解并获取其最小奇异值对应的奇异向量的代码为
 *   math::Matrix<double, 9, 9> V;
 *   math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &V);
 *   math::Vector<double, 9> f = V.col(8);
 *
 *
 *[奇异性约束]
 *  基础矩阵F的一个重要的性质是F是奇异的，秩为2，因此有一个奇异值为0。通过上述直接线性法求得
 *  矩阵不具有奇异性约束。常用的方法是将求得得矩阵投影到满足奇异约束得空间中。
 *  具体地，对F进行奇异值分解
 *               F = USV'
 *  其中S是对角矩阵，S=diag[sigma1, sigma2, sigma3]
 *  将sigma3设置为0，并重构F
 *                       [sigma1, 0,     ,0]
 *                 F = U [  0   , sigma2 ,0] V'
 *                       [  0   , 0      ,0]
 */
#include<opencv2/opencv.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<vector>

using namespace std;
using namespace cv;


Mat fundamental_8_point(const Mat& points1, const Mat points2)
{
	Mat A(8, 9, CV_64FC1);
	for (int i = 0; i < 8; i++)
	{
		Point3d p1(points1.at<double>(0, i), points1.at<double>(1, i), points1.at<double>(2, i));
		Point3d p2(points2.at<double>(0, i), points2.at<double>(1, i), points2.at<double>(2, i));
		A.at<double>(i, 0) = p1.x * p2.x;
		A.at<double>(i, 1) = p1.y * p2.x;
		A.at<double>(i, 2) = p2.x;
		A.at<double>(i, 3) = p1.x * p2.y;
		A.at<double>(i, 4) = p1.y * p2.y;
		A.at<double>(i, 5) = p2.y;
		A.at<double>(i, 6) = p1.x;
		A.at<double>(i, 7) = p1.y;
		A.at<double>(i, 8) = 1.0;
	}
	
	
	Mat cc, cc1,vv;
	SVD svd;
	svd.compute(A, cc, cc1, vv,4);
	vector<double> f = vv.row(8);
	Mat F(3, 3, CV_64FC1);

	F.at<double>(0, 0) = f[0];
	F.at<double>(0, 1) = f[1];
	F.at<double>(0, 2) = f[2];
	F.at<double>(1, 0) = f[3];
	F.at<double>(1, 1) = f[4];
	F.at<double>(1, 2) = f[5];
	F.at<double>(2, 0) = f[6];
	F.at<double>(2, 1) = f[7];
	F.at<double>(2, 2) = f[8];

	Mat dd, dd1, dd2;
	svd.compute(F, dd, dd1, dd2,4);
	Mat m = Mat::zeros(3, 3, CV_64FC1);
	m.at<double>(0, 0) = dd.at<double>(0);
	m.at<double>(1, 1) = dd.at<double>(1);
	F = dd1 * m * dd2;

	return F;
}

int main(int argc, char** argv)
{
	double a[24] = { 0.180123, 0.291429, -0.170373, 0.235952, 0.142122, -0.463158, 0.0801864, -0.179068,
										 -0.156584, 0.137662, 0.0779329, -0.164956, -0.216048, -0.132632, 0.0236417, 0.0837119,
										 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };

	Mat A = Mat::ones(1, 3, CV_32F);
	
	cv::Mat c = cv::Mat(3, 8, CV_64FC1, a);
	Mat pset1 = (Mat_< double > (3, 8) << 0.180123, 0.291429, -0.170373, 0.235952, 0.142122, -0.463158, 0.0801864, -0.179068,
		                                 -0.156584, 0.137662, 0.0779329, -0.164956, -0.216048, -0.132632, 0.0236417, 0.0837119,
		                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0);

	// 第二幅图像中的对应
	Mat pset2 = (Mat_<double>(3, 8) << 0.208264, 0.31484, -0.144499, 0.264461, 0.171033, -0.427861, 0.105406, -0.15257,
		                                 -0.035405, 0.267849, 0.190208, -0.0404422, -0.0961747, 0.00896567, 0.140966, 0.19645,
		                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0);
	Mat F = fundamental_8_point(pset1, pset2);

	std::cout << "Fundamental matrix after singularity constraint is:\n " << F << std::endl;

	std::cout << "Result should be: \n" << "-0.0315082 -0.63238 0.16121\n"
		<< "0.653176 -0.0405703 0.21148\n"
		<< "-0.248026 -0.194965 -0.0234573\n" << std::endl;

	return 0;
}

求解基础矩阵的另一种方法为基于RANSAC的方法，具体实现如下：

#include<opencv2/opencv.hpp>
#include<iostream>
#include<fstream>
#include<sstream>
#include<set>
#include<vector>
using namespace cv;
using namespace std;

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

typedef std::vector<Correspondence2D2D> Correspondences2D2D;

template<typename T>
T fastpow(T const& base, int exp)
{
	T ret = (exp == 0 ? T(1) : base);
	const int exp2 = exp / 2;
	int i = 1;
	for (; i < exp2; i = i * 2)
		ret = ret * ret;
	for (; i < exp; ++i)
		ret = ret * base;
	return ret;
}

/**
 * \description 用于RANSAC采样成功所需要的采样次数
 * @param p -- 内点的概率
 * @param K --拟合模型需要的样本个数，对应基础矩阵num_samples=8
 * @param z  -- 预期的采样成功的概率
 *                          log(1-z)
 *       需要的采样次数 M = -----------
 *                          log(1-p^K)
 * Example: For p = 50%, z = 99%, n = 8: M = log(0.001) / log(0.99609) = 1176.
 * 需要采样1176次从而保证RANSAC的成功率不低于0.99.
 * @return
 */
int calc_ransac_interation(double p, int K, double z = 0.99)
{
	double prob_all_good = fastpow(p, K);
	double num_iterations = std::log(1.0 - z) / std::log(1.0 - prob_all_good);

	return static_cast<int>(std::round(num_iterations));
}

/**
 * \description 给定基础矩阵和一对匹配点，计算匹配点的sampson 距离，用于判断匹配点是否是内点,
 * 计算公式如下：
 *              SD = (x'Fx)^2 / ( (Fx)_1^2 + (Fx)_2^2 + (x'F)_1^2 + (x'F)_2^2 )
 * @param F-- 基础矩阵
 * @param m-- 匹配对
 * @return
 */
double calc_sampson_distance(Mat &F, Correspondence2D2D const& m)
{
	double p2_F_p1 = 0.0;
	p2_F_p1 += m.p2[0] * (m.p1[0] * F.at<double>(0) + m.p1[1] * F.at<double>(1) + F.at<double>(2));
	p2_F_p1 += m.p2[1] * (m.p1[0] * F.at<double>(3) + m.p1[1] * F.at<double>(4) + F.at<double>(5));
	p2_F_p1 += 1.0 * (m.p1[0] * F.at<double>(6) + m.p1[1] * F.at<double>(7) + F.at<double>(8));
	p2_F_p1 *= p2_F_p1;

	double sum = 0.0;
	sum += fastpow(m.p1[0] * F.at<double>(0) + m.p1[1] * F.at<double>(1) + F.at<double>(2), 2);
	sum += fastpow(m.p1[0] * F.at<double>(3) + m.p1[1] * F.at<double>(4) + F.at<double>(5), 2);
	sum += fastpow(m.p2[0] * F.at<double>(0) + m.p2[1] * F.at<double>(3) + F.at<double>(6), 2);
	sum += fastpow(m.p2[0] * F.at<double>(1) + m.p2[1] * F.at<double>(4) + F.at<double>(7), 2);

	return p2_F_p1 / sum;
}

/**
 * \description 给定匹配对和基础矩阵，计算内点的个数
 * @param matches
 * @param F
 * @return
 */
std::vector<int> find_inliers(Correspondences2D2D const &matches, Mat &F, const double & thresh)
{
	const double squared_thresh = thresh * thresh;

	std::vector<int> inliers;

 	for (int i = 0; i < matches.size(); i++)
	{
		double error = calc_sampson_distance(F, matches[i]);

		if (error < squared_thresh)
		{
			inliers.push_back(i);
		}
	}
	return inliers;
}

/**
 * \description 8点发估计相机基础矩阵
 * @param pset1 -- 第一个视角的特征点
 * @param pset2 -- 第二个视角的特征点
 * @return 估计的基础矩阵
 */
Mat calc_fundamental_8_point(const Mat& points1, const Mat& points2)
{
	Mat A(8, 9, CV_64FC1);
	for (int i = 0; i < 8; i++)
	{
		Point3d p1(points1.at<double>(0, i), points1.at<double>(1, i), points1.at<double>(2, i));
		Point3d p2(points2.at<double>(0, i), points2.at<double>(1, i), points2.at<double>(2, i));
		A.at<double>(i, 0) = p1.x * p2.x;
		A.at<double>(i, 1) = p1.y * p2.x;
		A.at<double>(i, 2) = p2.x;
		A.at<double>(i, 3) = p1.x * p2.y;
		A.at<double>(i, 4) = p1.y * p2.y;
		A.at<double>(i, 5) = p2.y;
		A.at<double>(i, 6) = p1.x;
		A.at<double>(i, 7) = p1.y;
		A.at<double>(i, 8) = 1.0;
	}


	Mat cc, cc1, vv;
	SVD svd;
	svd.compute(A, cc, cc1, vv, 4);
	vector<double> f = vv.row(8);
	Mat F(3, 3, CV_64FC1);

	F.at<double>(0, 0) = f[0];
	F.at<double>(0, 1) = f[1];
	F.at<double>(0, 2) = f[2];
	F.at<double>(1, 0) = f[3];
	F.at<double>(1, 1) = f[4];
	F.at<double>(1, 2) = f[5];
	F.at<double>(2, 0) = f[6];
	F.at<double>(2, 1) = f[7];
	F.at<double>(2, 2) = f[8];

	Mat dd, dd1, dd2;
	svd.compute(F, dd, dd1, dd2, 4);
	Mat m = Mat::zeros(3, 3, CV_64FC1);
	m.at<double>(0, 0) = dd.at<double>(0);
	m.at<double>(1, 1) = dd.at<double>(1);
	F = dd1 * m * dd2;

	return F;
}

/**
 * \description 利用最小二乘法计算基础矩阵
 * @param matches--输入的匹配对 大于8对
 * @param F --基础矩阵
 */
void calc_fundamential_least_squares(Correspondences2D2D const &matches,Mat &F)
{
	if (matches.size() < 8)
		throw invalid_argument("At least 8 points requares");

	/* Create Nx9 matrix A. Each correspondence creates on row in A. */
	int n = matches.size();
	Mat A(n, 9, CV_64FC1);
	//vector<double> A(matches.size() * 9);
	for (int i = 0; i < matches.size(); ++i)
	{
		Correspondence2D2D const &  p = matches[i];
		
		A.at<double>(i, 0) = p.p2[0] * p.p1[0];
		A.at<double>(i, 1) = p.p2[0] * p.p1[1];
		A.at<double>(i, 2) = p.p2[0] * 1.0;
		A.at<double>(i, 3) = p.p2[1] * p.p1[0];
		A.at<double>(i, 4) = p.p2[1] * p.p1[1];
		A.at<double>(i, 5) = p.p2[1] * 1.0;
		A.at<double>(i, 6) = 1.0 * p.p1[0];
		A.at<double>(i, 7) = 1.0 * p.p1[1];
		A.at<double>(i, 8) = 1.0 * 1.0;
	}
	Mat vv;
	Mat cc, cc1;
	SVD::compute(A, cc, cc1, vv,4);


	vector<double> f = vv.row(8);
	Mat F1(3, 3, CV_64FC1);
	F1.at<double>(0, 0) = f[0];
	F1.at<double>(0, 1) = f[1];
	F1.at<double>(0, 2) = f[2];
	F1.at<double>(1, 0) = f[3];
	F1.at<double>(1, 1) = f[4];
	F1.at<double>(1, 2) = f[5];
	F1.at<double>(2, 0) = f[6];
	F1.at<double>(2, 1) = f[7];
	F1.at<double>(2, 2) = f[8];

	Mat U, S, V;
	SVD::compute(F1, S, U, V, 4);
	Mat S1 = Mat::zeros(3, 3, CV_64FC1);
	S1.at<double>(0, 0) = S.at<double>(0);
	S1.at<double>(1, 1) = S.at<double>(1);

	F = U * S1*V;
}

int main(int argc, char** argv)
{
	/**  加载归一化后的匹配对  **/
	Correspondences2D2D corr_all;
	std::ifstream in("correspondences.txt");
	assert(in.is_open());

	std::string line, word;
	int n_line = 0;
	while (getline(in, line))
	{
		std::stringstream stream(line);
		if (n_line == 0)
		{
			int n_corrs = 0;
			stream >> n_corrs;
			corr_all.resize(n_corrs);

			n_line++;
			continue;
		}
		if (n_line > 0)
		{
			stream >> corr_all[n_line - 1].p1[0] >> corr_all[n_line - 1].p1[1];
			stream >> corr_all[n_line - 1].p2[0] >> corr_all[n_line - 1].p2[1];
		}
		n_line++;

	}

	/** 计算采用次数 **/
	const float inlier_ratio = 0.5;
	const int n_sample = 8;
	int n_iterations = calc_ransac_interation(inlier_ratio, n_sample);

	// 用于判读匹配对是否为内点
	const double inlier_thresh = 0.0015;

	// ransac 最终估计的内点
	std::vector<int> best_inliers;

	std::cout << "RANSAC-F: Running for " << n_iterations
		      << " iterations, threshold " << inlier_thresh
		      << "..." << std::endl;

	for (int i = 0; i < n_iterations; i++)
	{
		//1.0 随机找到8对不重复的匹配点
		std::set<int> indices;
		while (indices.size() < 8)
		{
			int c = rand() % corr_all.size();
			//cout << c << endl;
			indices.insert(c);
		}

		Mat pset1(3, 8, CV_64FC1);
		Mat pset2(3, 8, CV_64FC1);
		std::set<int>::const_iterator iter = indices.cbegin();

		for (int j = 0; j < 8; j++, iter++)
		{
			Correspondence2D2D const & match = corr_all[*iter];

			pset1.at<double>(0, j) = match.p1[0];
			pset1.at<double>(1, j) = match.p1[1];
			pset1.at<double>(1, j) = 1.0;

			pset2.at<double>(0, j) = match.p2[0];
			pset2.at<double>(1, j) = match.p2[1];
			pset2.at<double>(1, j) = 1.0;
		}

		//2.0 8点法估计相机基础矩阵
		Mat F= calc_fundamental_8_point(pset1, pset2);

		//3.0 统计所有的内点个数
		std::vector<int> inlier_indices = find_inliers(corr_all, F, inlier_thresh);

		if (inlier_indices.size() > best_inliers.size())
		{
			best_inliers.swap(inlier_indices);
		}
	}

	Correspondences2D2D corr_f;
	for (int i = 0; i < best_inliers.size(); i++)
		corr_f.push_back(corr_all[best_inliers[i]]);

	//利用所有的内点进行最小二乘估计
	Mat F;
	calc_fundamential_least_squares(corr_f, F);
	cout << "inlier number: " << best_inliers.size() << std::endl;
	cout << "F\n: " << F << std::endl;


	return 0;
}