【三维重建】关键矩阵，基础矩阵，单应矩阵

立体视觉入门指南（2）：关键矩阵（本质矩阵，基础矩阵，单应矩阵）
源码地址同上个博客内容

自己按着博客推了下本质矩阵和基础矩阵是怎么来的：

下面两个是本质矩阵的分解和求解：

自己按着按着源码敲了下本质矩阵矩阵的程序推导：

本质矩阵
essential_solver.h

#ifndef SV3D_MATRIX_ESSENTIAL_H
#define SV3D_MATRIX_ESSENTIAL_H

#include "eigen_defs.h"

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

        enum SOLVE_TYPE
        {
            EIGHT_POINNTS = 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);

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

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

        /* 本质矩阵数据 */
        Mat3 data_;
    };
}

#endif

essential_solver.cpp

#ifndef SV3D_MATRIX_ESSENTIAL_H
#define SV3D_MATRIX_ESSENTIAL_H

#include "eigen_defs.h"

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

        enum SOLVE_TYPE
        {
            EIGHT_POINNTS = 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);

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

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

        /* 本质矩阵数据 */
        Mat3 data_;
    };
}

#endif

单应性矩阵求解：
homography_solver.h

/* -*-c++-*- StereoV3D - Copyright (C) 2021.
* Author	: Ethan Li<ethan.li.whu@gmail.com>
* https://github.com/ethan-li-coding/StereoV3DCode
*/

#ifndef SV3D_MATRIX_HOMOGRAPHY_H
#define SV3D_MATRIX_HOMOGRAPHY_H

#include "eigen_defs.h"


namespace  sv3d
{

	class HomographySolver
	{
	public:
		HomographySolver() = default;
		~HomographySolver() = default;
		
		/**
		 * \brief 单应性矩阵求解
		 * \param[in] p1 视图1上像素点齐次坐标
		 * \param[in] p2 视图2上像素点齐次坐标
		 */
		void Solve(const Mat3X p1, const Mat3X p2);

		/**
		 * \brief 获取单应性矩阵
		 * \return 单应性矩阵
		 */
		Mat3 Value();
	private:
		/**
		 * \brief 四点法求解单应性矩阵
		 * \param[in] p1 视图1上的像素点齐次坐标
		 * \param[in] p2 视图2上的像素点齐次坐标
		 */
		void Solve_FourPoints(const Mat3X p1, const Mat3X p2);

		/* 单应性矩阵数据 */
		Mat3 data_;
	};

}

#endif

homography_solver.cpp

#include "homography_solver.h"

void sv3d::HomographySolver::Solve(const Mat3X p1, const Mat3X p2)
{
    Solve_FourPoints(p1, p2);
}

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

void sv3d::HomographySolver::Solve_FourPoints(const Mat3X p1, const Mat3X p2)
{
    assert(p1.cols() >= 4);
    assert(p1.rows() == p2.rows());
    assert(p1.cols() == p2.cols());

    // 构建线性方程组Ah = b的系数矩阵A和矩阵b
    auto np = p1.cols();
    RMatXX a_mat(2 * np, 8), at(8, 2 * np), ata(8, 8);
    MatXX b_mat(2 * np, 1), atb(8, 1);
    for (int n = 0; n < np; n++)
    {
        const auto p1_x = p1.data()[3 * n], p1_y = p1.data()[3 * n + 1];
        const auto p2_x = p2.data()[3 * n], p2_y = p2.data()[3 * n + 1];
        auto dat = a_mat.data() + 8 * 2 * n;
        dat[0] = p1_x;
        dat[1] = p1_y;
        dat[2] = 1;
        dat[3] = dat[4] = dat[5] = 0;
        dat[6] = -p1_x * p2_x;
        dat[7] = -p1_y * p2_x;
        dat = a_mat.data() + 8 * (2 * n + 1);
        dat[0] = dat[1] = dat[2] = 0;
        dat[3] = p1_x;
        dat[4] = p1_y;
        dat[5] = 1;
        dat[6] = -p1_x * p2_y;
        dat[7] = -p1_y * p2_y;
        b_mat.data()[2 * n] = p2_x;
        b_mat.data()[2 * n + 1] = p2_y;
    }

    // 解 Ah = b
    at = a_mat.transpose();
    ata = at * a_mat;
    atb = at * b_mat;
    MatXX h(8, 1);
    h = ata.inverse() * atb;

    // 构造单应性矩阵
    data_ = Eigen::Map<const RMat3>(h.data());
    data_.data()[8] = 1;
}