【三维重建】关键矩阵，基础矩阵，单应矩阵
立体视觉入门指南（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;
}
