1对极几何
1.1本质矩阵
1.1.1几何约束与推导
如图所示，物体点 P P P，图像点 p 1 , p 2 p_1,p_2 p1,p2,相机中心 o 1 , o 2 o_1,o_2 o1,o2五点共面的关系称为对极几何。 o 1 , o 2 o_1,o_2 o1,o2连线称为基线，其与图像的交点称为 e 1 , e 2 e_1,e_2 e1,e2极点，图像点 p 1 , p 2 p_1,p_2 p1,p2与极点 e 1 , e 2 e_1,e_2 e1,e2组成的连线称为极线。
不妨假设图像点在相机坐标系的坐标为 p 1 , p 2 p_1,p_2 p1,p2，在三维空间中，由叉乘的几何意义可以得到一个垂直于平面的法向量，又有两个垂直的向量的点乘的结果为0，由对极几何的五点共面可以得到以下等式：
o 1 P 1 ⃗ . ( o 1 o 2 ⃗ × o 2 P 2 ⃗ ) = 0 (1) \vec{o_1P_1} .(\vec{o_1o_2}\times\vec{o_2P_2})=0\tag{1} o1P1.(o1o2×o2P2)=0(1)
假设 o 2 o_2 o2相机坐标系相对于 o 1 o_1 o1相机坐标系的旋转和平移为 R , t R,t R,t，不妨将坐标都统一到 o 1 o_1 o1相机坐标系，则 o 1 o 2 ⃗ = t , o 2 P 2 ⃗ = R P 2 ( 平移不影响向量方向 ) \vec{o_1o_2}=t,\vec{o_2P_2}=RP_2(平移不影响向量方向) o1o2=t,o2P2=RP2(平移不影响向量方向)则等式(1)可以写为：
P 1 T . ( t × R P 2 ) = 0 (2) P_1^T.(t\times{RP_2})=0\tag{2} P1T.(t×RP2)=0(2)
将叉乘写成反对称矩阵形式：
P 1 T . ( t ∧ R ) P 2 = 0 (3) P_1^T.(t^\wedge{R})P_2=0\tag{3} P1T.(t∧R)P2=0(3)
将图像点，相机中心，物体点共面方程称为对极约束。令 E = t ∧ R E=t^\wedge{R} E=t∧R，称 E E E为本质矩阵。
其中 $ P_1, P_2 $ 为归一化坐标（即相机坐标系下 $ z=1 $ 平面上的点）。
1.1.2 本质矩阵的性质
- 自由度：5 个（旋转 3 自由度 + 平移方向 2 自由度，平移尺度未定）
- 奇异值结构：通过 SVD 分解，$ E $ 的奇异值满足 σ 1 = σ 2 \sigma_1 = \sigma_2 σ1=σ2, σ 3 = 0 \sigma_3 = 0 σ3=0
- 分解方法：对 $ E $ 进行 SVD 分解 $ E = U \Sigma V^T $，可得 R = U W V T R = U W V^T R=UWVT 或 U W T V T U W^T V^T UWTVT，平移 t = ± U \mathbf{t} = \pm U t=±U
1.2基本矩阵
1.2.1 从本质矩阵到基本矩阵
基本矩阵 F F F 在像素坐标系中定义。设两相机的内参矩阵为 K 1 K_1 K1 和 K 2 K_2 K2，像素坐标 p 1 = K 1 P \mathbf{p}_1 = K_1 P p1=K1P、 p 2 = K 2 P 2 \mathbf{p}_2 = K_2 P_2 p2=K2P2，则等式(3)对极约束变为：
p 2 T ( K 2 − T E K 1 − 1 ) p 1 = 0 (4) \mathbf{p}_2^T (K_2^{-T} E K_1^{-1}) \mathbf{p}_1 = 0 \tag{4} p2T(K2−TEK1−1)p1=0(4)
由此定义基本矩阵：
F = K 2 − T E K 1 − 1 (5) F = K_2^{-T} E K_1^{-1}\tag{5} F=K2−TEK1−1(5)
直接表达为像素坐标间的约束：
p 2 T F p 1 = 0 (6) \mathbf{p}_2^T F \mathbf{p}_1 = 0\tag{6} p2TFp1=0(6)
2. 基本矩阵的性质
- 自由度：7 个（秩为 2 的 3×3 矩阵，尺度任意）
- 奇异值结构：秩为 2，两个非零奇异值无特定比例要求
2基本矩阵的求解过程
2.1八点法
原理：基于对极约束公式 x ′ T F x = 0 \mathbf{x'}^T F \mathbf{x} = 0 x′TFx=0，其中 x = ( u , v , 1 ) T \mathbf{x}=(u, v, 1)^T x=(u,v,1)T 和 x ′ = ( u ′ , v ′ , 1 ) T \mathbf{x'}=(u', v', 1)^T x′=(u′,v′,1)T 是两幅图像中的对应点。展开后得到一个线性方程：
u ′ u F 11 + u ′ v F 12 + u ′ F 13 + v ′ u F 21 + v ′ v F 22 + v ′ F 23 + u F 31 + v F 32 + F 33 = 0 (7) u'u F_{11} + u'v F_{12} + u' F_{13} + v'u F_{21} + v'v F_{22} + v' F_{23} + u F_{31} + v F_{32} + F_{33} = 0\tag{7} u′uF11+u′vF12+u′F13+v′uF21+v′vF22+v′F23+uF31+vF32+F33=0(7)
将每个对应点代入方程，构造齐次线性方程组 A f = 0 A \mathbf{f} = 0 Af=0，其中 A A A 是由对应点坐标组成的矩阵， f \mathbf{f} f 是 F F F 的向量化形式。当至少存在8对点时，通过奇异值分解（SVD）求解 A A A 的最小奇异值对应的右奇异向量作为 F F F 的估计。
秩约束修正：直接求解的 F F F 可能不满足秩为2的约束，需对 F F F 进行SVD分解 F = U Σ V T F = U \Sigma V^T F=UΣVT，将最小奇异值置零得到 Σ ′ \Sigma' Σ′，最终 F ′ = U Σ ′ V T F' = U \Sigma' V^T F′=UΣ′VT。
2.2七点法
原理：当仅7对点可用时，方程组 A f = 0 A \mathbf{f} = 0 Af=0 的解空间为二维，通解为 F = λ F 1 + ( 1 − λ ) F 2 F = \lambda F_1 + (1-\lambda) F_2 F=λF1+(1−λ)F2。通过秩约束 det ( F ) = 0 \text{det}(F) = 0 det(F)=0 得到关于 λ \lambda λ 的三次方程，可能产生1个或3个实数解，需进一步验证。
设7对点生成方程 A f = 0 A \mathbf {f} = 0 Af=0，通解为 f = α f 1 + β f 2 \mathbf{f} = \alpha \mathbf{f}_1 + \beta \mathbf{f}_2 f=αf1+βf2。代入秩约束：
det ( α F 1 + β F 2 ) = 0 (8) \det(\alpha F_1 + \beta F_2) = 0\tag{8} det(αF1+βF2)=0(8)
展开后得到形如 a α 3 + b α 2 β + c α β 2 + d β 3 = 0 a\alpha^3 + b\alpha^2\beta + c\alpha\beta^2 + d\beta^3 = 0 aα3+bα2β+cαβ2+dβ3=0 的三次方程，最多有3组实数解。
2.3提高精度的辅助手段
正如使用DLT估计单应矩阵一样，这里同样可以使用归一化、RANSAC、LM算法提高精度。在RANSAC可以使用对称极线距离判断内点。
计算对称极线距离误差
原理：基本矩阵应满足对极约束方程 x ′ T F x = 0 \mathbf{x'}^T F \mathbf{x} = 0 x′TFx=0。实际计算中，通过测量点到极线的几何距离评估误差。
步骤：
-
计算极线方程：
- 左图点 x i \mathbf{x}_i xi 在右图的极线： l i ′ = F x i l_i' = F \mathbf{x}_i li′=Fxi
- 右图点 x i ′ \mathbf{x}_i' xi′ 在左图的极线： l i = F T x i ′ l_i = F^T \mathbf{x}_i' li=FTxi′
-
计算点到极线距离：
d ( x ′ , l ′ ) = ∣ a ′ u ′ + b ′ v ′ + c ′ ∣ a ′ 2 + b ′ 2 (9) d(\mathbf{x}', l') = \frac{|a'u' + b'v' + c'|}{\sqrt{a'^2 + b'^2}}\tag{9} d(x′,l′)=a′2+b′2∣a′u′+b′v′+c′∣(9)其中 l ′ = [ a ′ , b ′ , c ′ ] T l' = [a', b', c']^T l′=[a′,b′,c′]T 为极线方程。\
-
对称极线距离：
将极线方程代入点到极线距离公式得到：
d sym = 1 2 ( ∣ x ′ T F x ∣ ( F x ) 1 2 + ( F x ) 2 2 + ∣ x T F T x ′ ∣ ( F T x ′ ) 1 2 + ( F T x ′ ) 2 2 ) (10) d_{\text{sym}} = \frac{1}{2} \left( \frac{|\mathbf{x'}^T F \mathbf{x}|}{\sqrt{(F\mathbf{x})_1^2 + (F\mathbf{x})_2^2}} + \frac{|\mathbf{x}^T F^T \mathbf{x'}|}{\sqrt{(F^T\mathbf{x'})_1^2 + (F^T\mathbf{x'})_2^2}} \right)\tag{10} dsym=21((Fx)12+(Fx)22∣x′TFx∣+(FTx′)12+(FTx′)22∣xTFTx′∣)(10)
理论上重投影点应位于极线上，即理论上重投影点到极线的距离为0.
2.4一些讨论
2.4.1八点法和七点法比较
1. 自由度分析
基本矩阵 $ F $ 是一个3×3的齐次矩阵，其自由度为7，原因如下：
- 齐次性约束：$ F $ 的尺度任意性导致1个自由度损失（例如，$ F $ 和 $ kF $ 表示同一几何约束）。
- 秩约束：由于 t \text{t} t^是反对称阵，展开计算其行列式为0，根据线性代数理论，反对称阵秩为偶数，所以$ E $ 和$ F $ 的秩都为2（即：行列式 $ \det(F) = 0 $），进一步损失1个自由度。
因此，理论上只需7个独立方程即可确定 $ F $（七点法）。
2. 七点法的局限性
七点法通过构建 $ 7 \times 9 $ 的齐次方程组 $ A\mathbf{f} = 0 $ 求解 $ F $。由于秩约束的存在，解空间为二维，需通过三次方程 $ \det(F) = 0 $ 筛选出唯一解。然而：
- 多解问题：三次方程可能产生1或3个实数解，需引入额外验证点选择正确解。
- 噪声敏感：若输入点噪声较大，七点法可能因解空间的不稳定性导致错误结果。
3.八点法的原理与优势
八点法通过 非齐次化处理 将自由度问题转化为线性最小二乘问题,避免七点法的多解问题，直接通过线性代数得到唯一解.
3算法实现与验证
算法实现在ComputeFundamentMatrix类中
ComputeFundamentMatrix.h：
#ifndef COMPUTE_FUNDAMENT_MATRIX_H
#define COMPUTE_FUNDAMENT_MATRIX_H
#include<vector>
#include<iostream>#include<opencv2/opencv.hpp>// RANSAC参数结构体
struct RansacConfig {float confidence = 0.9f; // 置信度float threshold = 5.0f; // 内点阈值（像素）int max_iterations = 100; // 安全最大迭代次数
};
class ComputeFundamentMatrix
{
public:ComputeFundamentMatrix() = default;~ComputeFundamentMatrix() = default;inline int setImgPoints(const std::vector<cv::Point2f>& pts1, const std::vector<cv::Point2f>& pts2){if (pts1.size()!=pts2.size() ||pts1.size()<7){return 0;}else{m_pts1 = pts1;m_pts2 = pts2;}}int run();cv::Mat getFMatrix(){if (m_final_F.empty()){return cv::Mat();}else{return m_final_F;}}void getInlierPts(std::vector<uchar>&inlier_indices){if (!m_final_inliers.empty()){inlier_indices = m_final_inliers;}}
private:// 归一化坐标（Hartley的预处理方法）void normalizePoints(const std::vector<cv::Point2f>& points,std::vector<cv::Point2f>& normalized_points,cv::Mat& T);//7点法// 7点法核心算法（返回最多3个解）std::vector<cv::Mat> computeFundamenBy7Points(const std::vector<cv::Point2f>& pts1,const std::vector<cv::Point2f>& pts2);//利用8点法计算基本矩阵(最少8个点)cv::Mat computeFundamentBy8Points(const std::vector<cv::Point2f>& pts1,const std::vector<cv::Point2f>& pts2);std::vector<cv::Point2f> m_pts1, m_pts2;//重投影误差（重投影点到极线距离）double computeEpipolarError(const cv::Mat& F,const cv::Point2f& pt1,const cv::Point2f& pt2);// 核心RANSAC实现（结合8点法）cv::Mat ransacFundamentalMatrix(const std::vector<cv::Point2f>& pts1,const std::vector<cv::Point2f>& pts2,const RansacConfig& config,std::vector<uchar>& best_inliers,int& num_inliers);cv::Mat m_final_F{};std::vector<uchar>m_final_inliers{};};#endif // !COMPUTE_FUNDAMENT_MATRIX_HComputeFundamentMatrix.cpp：
#include "ComputeFundamentMatrix.h"
#include<random>
#include <vector>
#include <unsupported/Eigen/Polynomials> // 用于求解三次方程
//LM
/*
* #include <opencv2/core.hpp>
#include <unsupported/Eigen/LevenbergMarquardt>struct Functor {const std::vector<cv::Point2f>& pts1, &pts2;Functor(const std::vector<cv::Point2f>& p1, const std::vector<cv::Point2f>& p2) : pts1(p1), pts2(p2) {}int operator()(const Eigen::VectorXd& f_vec, Eigen::VectorXd& errors) const {cv::Mat F(3,3,CV_64F, (void*)f_vec.data());F /= F.at<double>(2,2); // 归一化最后一元素for (size_t i=0; i<pts1.size(); ++i) {cv::Mat x1 = (cv::Mat_<double>(3,1) << pts1[i].x, pts1[i].y, 1.0);cv::Mat x2 = (cv::Mat_<double>(3,1) << pts2[i].x, pts2[i].y, 1.0);cv::Mat Fx1 = F * x1;cv::Mat FTx2 = F.t() * x2;double num = std::abs(x2.dot(Fx1));double denom = cv::norm(Fx1.rowRange(0,2)) + 1e-6;errors(2*i) = num / denom;num = std::abs(x1.dot(FTx2));denom = cv::norm(FTx2.rowRange(0,2)) + 1e-6;errors(2*i+1) = num / denom;}return 0;}int df(const Eigen::VectorXd& f_vec, Eigen::MatrixXd& jacobian) {// 实现雅可比矩阵解析计算（此处简化为数值差分）const double eps = 1e-6;Eigen::VectorXd f_plus = f_vec;Eigen::VectorXd errors_plus(errors.size());for (int i=0; i<f_vec.size(); ++i) {f_plus[i] += eps;this->operator()(f_plus, errors_plus);jacobian.col(i) = (errors_plus - errors) / eps;f_plus[i] = f_vec[i];}return 0;}
};cv::Mat refineFundamentalLM(const cv::Mat& F_initial, const std::vector<cv::Point2f>& inliers1,const std::vector<cv::Point2f>& inliers2) {// 将F矩阵转换为向量Eigen::VectorXd f_vec(9);cv::Mat F_normalized = F_initial / F_initial.at<double>(2,2);memcpy(f_vec.data(), F_normalized.ptr<double>(), 9*sizeof(double));// 配置LM优化器Eigen::LevenbergMarquardt<Functor> lm(Functor(inliers1, inliers2));lm.parameters.maxfev = 100; // 最大函数评估次数lm.parameters.xtol = 1e-6; // 参数变化阈值// 执行优化int ret = lm.minimize(f_vec);// 重构矩阵cv::Mat F_refined(3,3,CV_64F, f_vec.data());return F_refined / F_refined.at<double>(2,2);
}*/
// 归一化坐标（Hartley的预处理方法）
void ComputeFundamentMatrix::normalizePoints
(const std::vector<cv::Point2f>& points,std::vector<cv::Point2f>& normalized_points,cv::Mat& T)
{const int N = points.size();cv::Point2f centroid(0, 0);// 计算质心for (const auto& p : points) {centroid += p;}centroid /= N;// 计算缩放因子（使平均距离到质心为sqrt(2)）float mean_dist = 0.0;for (const auto& p : points) {mean_dist += cv::norm(p - centroid);}mean_dist /= N;const float scale = sqrt(2.0) / mean_dist;// 构建归一化矩阵T = (cv::Mat_<float>(3, 3) << scale, 0, -scale * centroid.x,0, scale, -scale * centroid.y,0, 0, 1);// 应用归一化变换normalized_points.resize(N);for (int i = 0; i < N; ++i) {const float x = T.at<float>(0, 0) * points[i].x + T.at<float>(0, 1) * points[i].y + T.at<float>(0, 2);const float y = T.at<float>(1, 0) * points[i].x + T.at<float>(1, 1) * points[i].y + T.at<float>(1, 2);normalized_points[i] = cv::Point2f(x, y);}
}std::vector<cv::Mat> ComputeFundamentMatrix::computeFundamenBy7Points(const std::vector<cv::Point2f>& pts1, const std::vector<cv::Point2f>& pts2)
{// 输入检查const int N = pts1.size();if (N != 7 || pts2.size() != 7) {throw std::runtime_error("Exactly 7 point pairs required");}// 步骤1：坐标归一化std::vector<cv::Point2f> norm_pts1, norm_pts2;cv::Mat T1, T2;normalizePoints(pts1, norm_pts1, T1);normalizePoints(pts2, norm_pts2, T2);// 步骤2：构建7x9系数矩阵Acv::Mat A(7, 9, CV_32F);for (int i = 0; i < 7; ++i) {const float u1 = norm_pts1[i].x;const float v1 = norm_pts1[i].y;const float u2 = norm_pts2[i].x;const float v2 = norm_pts2[i].y;A.at<float>(i, 0) = u2 * u1;A.at<float>(i, 1) = u2 * v1;A.at<float>(i, 2) = u2;A.at<float>(i, 3) = v2 * u1;A.at<float>(i, 4) = v2 * v1;A.at<float>(i, 5) = v2;A.at<float>(i, 6) = u1;A.at<float>(i, 7) = v1;A.at<float>(i, 8) = 1.0f;}// 步骤3：SVD求解解空间cv::Mat U, S, Vt;cv::SVDecomp(A, S, U, Vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 提取最后两个奇异向量作为基解cv::Mat F1 = Vt.row(7).reshape(0, 3); // 第8行（索引从0开始）cv::Mat F2 = Vt.row(8).reshape(0, 3); // 第9行// 步骤4：构造三次方程 det(αF1 + (1-α)F2) = 0// 展开行列式得到三次多项式系数cv::Mat F_alpha = F1.clone();std::vector<double> coeffs(4, 0.0);for (int i = 0; i < 3; ++i) {for (int j = 0; j < 3; ++j) {const double a = F1.at<float>(i, j);const double b = F2.at<float>(i, j);// 计算各阶系数贡献coeffs[0] += a * b * b; // α^3项coeffs[1] += a * a * b - 2 * a * b * b;coeffs[2] += b * b * b - 2 * a * a * b + a * a * a;coeffs[3] += -b * b * b; // 常数项}}// 使用Eigen求解三次方程Eigen::PolynomialSolver<double, 3> solver;Eigen::Vector4d poly_coeffs(coeffs[3], coeffs[2], coeffs[1], coeffs[0]);solver.compute(poly_coeffs);std::vector<cv::Mat> solutions;Eigen::PolynomialSolver<double, 3>::RootsType roots = solver.roots();// 遍历所有根，筛选实根for (int i = 0; i < roots.rows(); ++i) {if (std::abs(roots[i].imag()) < 1e-6) { // 仅保留实根const double alpha = roots[i].real();// 计算F矩阵：F = αF1 + (1-α)F2cv::Mat F = alpha * F1 + (1 - alpha) * F2;// 强制秩为2约束cv::Mat U_F, S_F, Vt_F;cv::SVDecomp(F, S_F, U_F, Vt_F, cv::SVD::MODIFY_A);S_F.at<float>(2) = 0; // 将第三个奇异值置零F = U_F * cv::Mat::diag(S_F) * Vt_F;// 反归一化：F = T2^T * F * T1F = T2.t() * F * T1;F /= F.at<float>(2, 2); // 归一化最后一元素solutions.push_back(F.clone());}}return solutions;
}// 8点法核心算法
cv::Mat ComputeFundamentMatrix::computeFundamentBy8Points
(const std::vector<cv::Point2f>& pts1,const std::vector<cv::Point2f>& pts2)
{// 输入检查const int N = pts1.size();if (N < 8 || pts2.size() != N) {throw std::runtime_error("At least 8 point pairs required");}// 步骤1：坐标归一化std::vector<cv::Point2f> norm_pts1, norm_pts2;cv::Mat T1, T2;normalizePoints(pts1, norm_pts1, T1);normalizePoints(pts2, norm_pts2, T2);// 步骤2：构建系数矩阵Acv::Mat A(N, 9, CV_32F);for (int i = 0; i < N; ++i) {const float u1 = norm_pts1[i].x;const float v1 = norm_pts1[i].y;const float u2 = norm_pts2[i].x;const float v2 = norm_pts2[i].y;A.at<float>(i, 0) = u2 * u1;A.at<float>(i, 1) = u2 * v1;A.at<float>(i, 2) = u2;A.at<float>(i, 3) = v2 * u1;A.at<float>(i, 4) = v2 * v1;A.at<float>(i, 5) = v2;A.at<float>(i, 6) = u1;A.at<float>(i, 7) = v1;A.at<float>(i, 8) = 1.0f;}// 步骤3：SVD求解最小二乘解cv::Mat U, S, Vt;cv::SVDecomp(A, S, U, Vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);cv::Mat F_normalized = Vt.row(8).reshape(0, 3); // 取最后一行的最小奇异向量// 步骤4：强制秩为2约束cv::Mat U_F, S_F, Vt_F;cv::SVDecomp(F_normalized, S_F, U_F, Vt_F, cv::SVD::MODIFY_A);S_F.at<float>(2) = 0; // 将第三个奇异值置零cv::Mat F_rank2 = U_F * cv::Mat::diag(S_F) * Vt_F;// 步骤5：反归一化cv::Mat F = T2.t() * F_rank2 * T1;// 归一化F矩阵F /= F.at<float>(2, 2);return F;
}
double ComputeFundamentMatrix::computeEpipolarError
(const cv::Mat& F,const cv::Point2f& pt1,const cv::Point2f& pt2)
{double total_error = 0.0;//for (size_t i = 0; i < pts1.size(); ++i) {cv::Mat x1 = (cv::Mat_<float>(3, 1) << pt1.x, pt1.y, 1.0);cv::Mat x2 = (cv::Mat_<float>(3, 1) << pt2.x, pt2.y, 1.0);//std::cout << F.type() << std::endl;//std::cout << x1.type() << std::endl;// 计算右图极线方程 l2 = F * x1cv::Mat l2 = F * x1;float a2 = l2.at<float>(0), b2 = l2.at<float>(1), c2 = l2.at<float>(2);float dist2 = std::abs(a2 * pt2.x + b2 * pt2.y + c2) / cv::sqrt(a2 * a2 + b2 * b2);// 计算左图极线方程 l1 = F^T * x2cv::Mat l1 = F.t() * x2;float a1 = l1.at<float>(0), b1 = l1.at<float>(1), c1 = l1.at<float>(2);float dist1 = std::abs(a1 * pt1.x + b1 * pt1.y + c1) / cv::sqrt(a1 * a1 + b1 * b1);total_error += (dist1 + dist2);}// return total_error / double(2 * pts1.size()); // 返回平均对称误差return total_error / double(2.0); // 返回平均对称误差}
// 核心RANSAC实现（结合8点法）
cv::Mat ComputeFundamentMatrix::ransacFundamentalMatrix(const std::vector<cv::Point2f>& pts1,const std::vector<cv::Point2f>& pts2,const RansacConfig& config,std::vector<uchar>& best_inliers,int& num_inliers)
{const int N = pts1.size();if (N < 8) throw std::runtime_error("至少需要8个匹配点");// 随机数生成器std::random_device rd;std::mt19937 rng(rd());std::uniform_int_distribution<int> dist(0, N - 1);// 最佳模型参数cv::Mat best_F;num_inliers = 0;best_inliers.resize(N, 0);// 动态迭代次数计算int updated_iterations = config.max_iterations;int iter = 0;while (iter++ < updated_iterations) {// 1. 随机采样8个点std::vector<cv::Point2f> sample1(8), sample2(8);std::vector<int> indices(8);for (int i = 0; i < 8; ++i) {int idx = dist(rng);indices[i] = idx; // 记录索引用于去重sample1[i] = pts1[idx];sample2[i] = pts2[idx];}// 2. 使用8点法计算F矩阵cv::Mat F;F = computeFundamentBy8Points(sample1, sample2);//try {// F = computeFundamentBy8Points(sample1, sample2);//}//catch (...) {// continue; // 跳过无效样本//}// 3. 评估内点数量std::vector<uchar> current_inliers(N, 0);int current_count = 0;for (int i = 0; i < N; ++i) {// 计算对称极线距离double distance = computeEpipolarError(F, pts1[i], pts2[i]);if (distance < config.threshold) {current_inliers[i] = 1;current_count++;}}// 4. 更新最佳模型if (current_count > num_inliers) {num_inliers = current_count;best_inliers = current_inliers;best_F = F.clone();// 动态更新迭代次数float inlier_ratio = current_count / float(N);updated_iterations = std::min(int(log(1 - config.confidence) /log(1 - pow(inlier_ratio, 8))),config.max_iterations);}}// 5. 用所有内点重新估计F矩阵std::vector<cv::Point2f> inlier_pts1, inlier_pts2;for (int i = 0; i < N; ++i) {if (best_inliers[i]) {inlier_pts1.push_back(pts1[i]);inlier_pts2.push_back(pts2[i]);}}if (inlier_pts1.size() >= 8) {best_F = computeFundamentBy8Points(inlier_pts1, inlier_pts2);}return best_F;
}
int ComputeFundamentMatrix::run()
{RansacConfig config;//std::vector<uchar> best_inliers;int num_inliers;m_final_F = ransacFundamentalMatrix(m_pts1, m_pts2, config, m_final_inliers, num_inliers);return 1;
}main函数：
// 求解基本矩阵.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//#include <iostream>
#include<opencv2/opencv.hpp>
#include"ComputeFundamentMatrix.h"
//寻找特征点
static void findFeaturePoints(const cv::Mat& img1,const cv::Mat& img2, std::vector<cv::Point2f> &keypoints1, std::vector<cv::Point2f>& keypoints2 )
{//using namespace cv;//using namespace std;cv::Mat g1(img1, cv::Rect(0, 0, img1.cols, img1.rows)); // init roicv::Mat g2(img2, cv::Rect(0, 0, img2.cols, img2.rows));cvtColor(g1, g1, cv::COLOR_BGR2GRAY);cvtColor(g2, g2, cv::COLOR_BGR2GRAY);std::vector<cv::KeyPoint> keypoints_roi, keypoints_img; /* keypoints found using SIFT */cv::Mat descriptor_roi, descriptor_img; /* Descriptors for SIFT */std::vector<cv::DMatch> matches, good_matches;//cv::Ptr<cv::SIFT> sift = cv::SIFT::create();//Ptr<cv::ORB>orb_feature = ORB::create(1000);//Ptr<cv::BRISK> brisk_feature = BRISK::create(1000);cv::Ptr<cv::AKAZE> akaze_feature = cv::AKAZE::create();int i, dist = 80;akaze_feature->detectAndCompute(g1, cv::Mat(), keypoints_roi, descriptor_roi); /* get keypoints of ROI image */akaze_feature->detectAndCompute(g2, cv::Mat(), keypoints_img, descriptor_img); /* get keypoints of the image *///FlannBasedMatcher matcher; /* FLANN based matcher to match keypoints *///matcher.match(descriptor_roi, descriptor_img, matches); //实现描述符之间的匹配cv::Ptr<cv::DescriptorMatcher> matche = cv::DescriptorMatcher::create("BruteForce-Hamming");matche->match(descriptor_roi, descriptor_img, matches);double max_dist = 0; double min_dist = 5000;//-- Quick calculation of max and min distances between keypointsfor (int i = 0; i < descriptor_roi.rows; i++){double dist = matches[i].distance;if (dist < min_dist) min_dist = dist;if (dist > max_dist) max_dist = dist;}// 特征点筛选for (i = 0; i < descriptor_roi.rows; i++){if (matches[i].distance < 5 * min_dist){good_matches.push_back(matches[i]);}}printf("%ld no. of matched keypoints in right image\n", good_matches.size());/* Draw matched keypoints */cv::Mat img_matches;//绘制匹配drawMatches(img1, keypoints_roi, img2, keypoints_img,good_matches, img_matches, cv::Scalar::all(-1),cv::Scalar::all(-1), std::vector<char>(),cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);cv::namedWindow("matches_image", cv::WINDOW_NORMAL);imshow("matches_image", img_matches);cv::waitKey(0);for (i = 0; i < good_matches.size(); i++){keypoints1.push_back(keypoints_img[good_matches[i].trainIdx].pt);keypoints2.push_back(keypoints_roi[good_matches[i].queryIdx].pt);}
}
//可视化对极几何的结果
static void visualizeEpipolarLines(cv::Mat& img1, cv::Mat& img2,const std::vector<cv::Point2f>& pts1,const std::vector<cv::Point2f>& pts2,const cv::Mat& F,int num_samples = 10)
{cv::RNG rng(12345);std::vector<int> indices;for (int i = 0; i < num_samples; ++i)indices.push_back(rng.uniform(0, pts1.size()));// 计算右图极线std::vector<cv::Vec3f> lines2;cv::computeCorrespondEpilines(pts1, 1, F, lines2); // 1表示左图点// 在右图绘制极线和点for (int idx : indices) {cv::Scalar color(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));cv::line(img2,cv::Point(0, -lines2[idx][2] / lines2[idx][1]),cv::Point(img2.cols, -(lines2[idx][2] + lines2[idx][0] * img2.cols) / lines2[idx][1]),color, 1);cv::circle(img2, pts2[idx], 3, color, -1);}// 显示图像cv::imshow("Epipolar Lines", img2);cv::waitKey(0);
}int main()
{cv::Mat img1 = cv::imread("7.jpg");cv::Mat img2 = cv::imread("8.jpg");if (img1.empty()||img2.empty()){return -1;}std::vector<cv::Point2f> keypoints1{}, keypoints2{};findFeaturePoints(img1, img2, keypoints1, keypoints2);std::cout << "keypointssize:" << keypoints1.size() << std::endl;ComputeFundamentMatrix cfm;cfm.setImgPoints(keypoints1, keypoints2);cfm.run();cv::Mat F_matrix = cfm.getFMatrix();std::cout <<"基本矩阵：\n" << F_matrix << std::endl;std::vector<uchar> inlier_indices;visualizeEpipolarLines(img1, img2, keypoints1, keypoints2, F_matrix,keypoints1.size()/3);std::cout << "Hello World!\n";
}极线匹配效果：
参考
《多视图几何学》
《orbslam》
《计算机视觉算法与应用》
1
slam14讲
