本质矩阵解算code

最新推荐文章于 2024-07-17 05:47:18 发布

三维重建及点云

最新推荐文章于 2024-07-17 05:47:18 发布

阅读量235

点赞数 1

分类专栏：三维重建文章标签：矩阵 c++ 算法

本文链接：https://blog.csdn.net/vegetablemwj/article/details/125618480

版权

三维重建专栏收录该内容

3 篇文章 0 订阅

订阅专栏

本质矩阵解算code

主函数

#include <chrono>
#include <iostream>
#include <string>
#include "eigen_defs.h"
#include "datasets.h"
#include "essential_solver.h"

using namespace std;
using namespace sv3d;
using namespace std::chrono;

void TestEssentialSolving();

int main()
{
	cout << "测试本质矩阵解算" << endl;
	TestEssentialSolving();
	return 0;

}
void TestEssentialSolving()
{
	//构造同名点集
	sv3d::SimulativeStereoDataset datasets;
	Mat3X p1, p2;
	const unsigned kPairsCount = 20;
	datasets.GenarateHomonymyPairs(kPairsCount, p1, p2);
	cout << kPairsCount << " pairs of homonymous points have been generated!" << endl << endl;

	/*解算本质矩阵*/
	cout << "开始解算本质矩阵" << endl;
	auto start = steady_clock::now();

	EssentialSolver EssentialSolver;
	EssentialSolver.Solve(p1, p2, datasets.cam1.K_, datasets.cam2.K_, EssentialSolver::EIGHT_POINTS);
	Mat3 Es = EssentialSolver.Value();

	auto end = steady_clock::now();
	auto tt = duration_cast<microseconds>(end - start);

	double diff = 0.0;
	double scale = datasets.E.data()[0] / Es.data()[0];
	Es *= scale;
	for (int i = 0; i < 9; i++) {
		diff += abs(Es.data()[i] - datasets.E.data()[i]);
	}
	diff /= 9;
	cout << "Done! Solving Mean Error = " << diff << "   Timing: " << tt.count() / 1000.0 << "ms" << endl << endl;
}

生成模拟双目点

#include "datasets.h"
#include <random>

void sv3d::SimulativeStereoDataset::GenarateHomonymyPairs(const unsigned& k, Mat3X& p1, Mat3X& p2)
{
	// 在立体相机的空间内随机生成k个空间点
	const double kDepthMin = 300., kDepthMax = 500.;

	// 在最小深度平面计算四个角点的世界坐标（世界坐标系=左相机坐标系）
	Vec3 x1(0, 0, kDepthMin), x2(0, h - 1, kDepthMin),
		x3(w - 1, 0, kDepthMin), x4(w - 1, h - 1, kDepthMin);
	auto P1 = cam1.TransformPointI2W(x1);
	auto P2 = cam1.TransformPointI2W(x2);
	auto P3 = cam1.TransformPointI2W(x3);
	auto P4 = cam1.TransformPointI2W(x4);

	// 生成一个空间立方体，在立方体范围内随机生成空间点
	const auto z_min(kDepthMin), z_max(kDepthMax);
	const auto x_min = (std::min)(P1.data()[0], P2.data()[0]);
	const auto x_max = (std::max)(P3.data()[0], P4.data()[0]);
	const auto y_min = (std::min)(P1.data()[0], P3.data()[0]);
	const auto y_max = (std::max)(P2.data()[0], P4.data()[0]);

	// 随机生成k个空间点
	std::random_device rd;
	std::mt19937 gen(rd());
	std::uniform_real_distribution<double> rand(0.0f, 1.0f);

	const auto x_range = x_max - x_min;
	const auto y_range = y_max - y_min;
	const auto z_range = z_max - z_min;
	p1.resize(3, k);
	p2.resize(3, k);
	for (unsigned n = 0; n < k; n++) {
		Vec3 w;
		w[0] = rand(gen) * x_range + x_min;
		w[1] = rand(gen) * y_range + y_min;
		w[2] = rand(gen) * z_range + z_min;

		Vec2 x1 = cam1.TransformPointW2I(w);
		Vec2 x2 = cam2.TransformPointW2I(w);

		p1.data()[3 * n] = x1[0]; p1.data()[3 * n + 1] = x1[1]; p1.data()[3 * n + 2] = 1.;
		p2.data()[3 * n] = x2[0]; p2.data()[3 * n + 1] = x2[1]; p2.data()[3 * n + 2] = 1.;
	}
}

datasets.h

#pragma once
#include "eigen_defs.h"
#include "camera.h"

namespace sv3d
{
	struct SimulativeStereoDataset
	{
		unsigned w, h;		// 影像宽高
		Camera	cam1, cam2;	// 两个相机

		Mat3 E, F;

		SimulativeStereoDataset()
		{
			// 模拟构造一组双目系统
			w = 640; h = 480;
			Mat3 K1, K2, R, RI;
			Vec3 t0, t;
			K1 << 5.3398796382502758e+02, 0., 3.2838647583744523e+02,
				0., 5.2871083166023755e+02, 2.3684273091753437e+02,
				0., 0., 1.;
			K2 << 5.3398796382502758e+02, 0., 3.1377033111753229e+02,
				0., 5.2871083166023755e+02, 2.4187045690014719e+02,
				0., 0., 1.;
			R << 9.9997149579080535e-01, 4.8210116286829859e-03, 5.8108048302649620e-03,
				-4.8866671827332113e-03, 9.9992378062933918e-01, 1.1338139872769284e-02,
				-5.7557006302035411e-03, -1.1366212157327780e-02, 9.9991883727203090e-01;
			t << -3.3427086946183784e+00, 4.6825941478185688e-02, 3.6523737018390699e-03;

			RI.setIdentity();//初始化缩放的标识矩阵
			t0.setZero();
			cam1 = Camera(K1, RI, t0);
			cam2 = Camera(K2, R, t);

			//求解本质矩阵
			Mat3 tx;
			tx << 0., -cam2.t_[2], cam2.t_[1],
				cam2.t_[2], 0., -cam2.t_[0],
				-cam2.t_[1], cam2.t_[0], 0;
			E = tx * R;


			//求解基础矩阵
			F = cam2.K_.inverse().transpose() * E * cam1.K_.inverse();

		}

		/**
 * \brief 生成同名点对
 * \param k[in]		要生成的同名点对数量
 * \param p1[in]	相机1上的像素点
 * \param p2[in]	相机2上的像素点
 */
		void GenarateHomonymyPairs(const unsigned& k, Mat3X& p1, Mat3X& p2);
	};
}

essential_solver.h

#pragma once
#include "eigen_defs.h"

namespace sv3d
{
	class EssentialSolver
	{
	public:
		EssentialSolver() = default;
		~EssentialSolver() = default;

		enum SOLVE_TYPE
		{
			EIGHT_POINTS = 0,
		};
				/**
		 * \brief 本质矩阵求解
		 * \param p1[in] 视图1上像素点齐次坐标
		 * \param p2[in] 视图2上像素点齐次坐标
		 * \param k1_mat[in] 相机1内参矩阵
		 * \param k2_mat[in] 相机2内参矩阵
		 * \param solver_type[in] 求解算法类型
		 */
		void Solve(const Mat3X p1, const Mat3X p2, const Mat3 k1_mat, const Mat3 k2_mat, const SOLVE_TYPE& solver_type);

		/**
 * \brief 本质矩阵求解
 * \param x1[in] 视图1上的归一化像素点齐次坐标(x = k_inv*p)
 * \param x2[in] 视图2上的归一化像素点齐次坐标
 * \param solver_type[in] 求解算法类型
 */
		void Solve(const Mat3X x1, const Mat3X x2, const SOLVE_TYPE& solver_type);

		/*
		获取本质矩阵
		*/
		Mat3 Value();

	private:
		/*八点法求解本质矩阵
		x1：视图1上的归一化像素点齐次坐标
		x2：视图2上的归一化像素点齐次坐标*/
		void Solve_EightPoints(const Mat3X x1, const Mat3X x2);
		/* 本质矩阵数据 */
		Mat3 data_;
	};
}

essential_solver.cpp

#include "eigen_defs.h"
#include "essential_solver.h"

void sv3d::EssentialSolver::Solve(const Mat3X p1, const Mat3X p2, const Mat3 k1_mat, const Mat3 k2_mat, const SOLVE_TYPE & solver_type)
{
	assert(p1.cols() >= 8);
	assert(p1.rows() == p2.rows());
	assert(p1.cols() == p2.cols());

	// 通过内参矩阵k将p转换到x，x = k_inv*p
	Mat3X x1(3, p1.cols()), x2(3, p2.cols());

	x1 = k1_mat.inverse() * p1;
	x2 = k2_mat.inverse() * p2;

	// 求解
	Solve(x1, x2, solver_type);
}

void sv3d::EssentialSolver::Solve(const Mat3X x1, const Mat3X x2, const SOLVE_TYPE & solver_type)
{
	switch (solver_type) {
	case EIGHT_POINTS:
		Solve_EightPoints(x1, x2);
	default:
		break;
	}
}

sv3d::Mat3 sv3d::EssentialSolver::Value()
{
	return data_;
}


void sv3d::EssentialSolver::Solve_EightPoints(const Mat3X x1, const Mat3X x2)
{
	assert(x1.cols() >= 8);
	assert(x1.rows() == x2.rows());
	assert(x1.cols() == x2.cols());

	// 构建线性方程组的系数矩阵A
	auto np = x1.cols();
	RMatX9 a_mat(np, 9);
	for (int n = 0; n < np; n++) {
		const auto x1_x = x1.data()[3 * n];
		const auto x1_y = x1.data()[3 * n + 1];
		const auto x2_x = x2.data()[3 * n];
		const auto x2_y = x2.data()[3 * n + 1];
		const auto dat = a_mat.data() + 9 * n;
		dat[0] = x1_x * x2_x; dat[1] = x2_x * x1_y; dat[2] = x2_x;
		dat[3] = x1_x * x2_y; dat[4] = x1_y * x2_y; dat[5] = x2_y;
		dat[6] = x1_x; dat[7] = x1_y; dat[8] = 1;
	}

	// 求解ATA的最小特征值对应的特征向量即矢量 e
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 9, 9>> solver(a_mat.transpose()*a_mat);
	const Vec9 e = solver.eigenvectors().col(0);

	// 矢量 e 构造本质矩阵 E
	data_ = Eigen::Map<const RMat3>(e.data());

	// 调整 E 矩阵使满足内在性质：奇异值为[σ σ 0]
	Eigen::JacobiSVD<Mat3> usv(data_, Eigen::ComputeFullU | Eigen::ComputeFullV);
	auto s = usv.singularValues();
	const auto a = s[0];
	const auto b = s[1];
	s << (a + b) / 2.0, (a + b) / 2.0, 0.0;
	data_ = usv.matrixU() * s.asDiagonal() * usv.matrixV().transpose();
}