1 引言
在上一篇文章《最小二乘问题详解13：对极几何中本质矩阵求解》中，我们系统地探讨了在相机内参已知的前提下，如何从两视图的2D-2D特征匹配中恢复相机的相对位姿。我们首先建立了对极几何的数学模型，并推导出核心约束——对极约束 \(\tilde{\mathbf{x}}_2^\top \mathbf{E} \tilde{\mathbf{x}}_1 = 0\)。在此基础上，我们详细分析了两种求解本质矩阵 \(\mathbf{E}\) 的方法：经典的8点线性算法，它通过求解一个齐次线性系统获得初值；以及基于Sampson误差的非线性优化，它能有效提升解的几何精度。
然而，上述方法都建立在一个关键且脆弱的假设之上：所有的输入匹配点都是正确的（即内点）。在真实的计算机视觉应用中，这一假设几乎从不成立。由于重复纹理、遮挡、运动模糊或特征描述子的局限性，特征匹配阶段不可避免地会引入大量错误的对应关系，即外点（outliers）。这些外点完全不遵循由真实相对位姿所决定的对极几何，若直接将包含外点的数据送入8点算法或非线性优化器，其结果将会被严重污染，甚至完全失效。
因此，在实际的视觉系统应用中，必须采用更为鲁棒的估计方法，以保证在存在大量异常数据的情况下，依然能够准确地估计出模型参数。在多视图几何领域，最著名、最有效的鲁棒估计算法莫过于 RANSAC（Random Sample Consensus，随机抽样一致性）。
RANSAC 本身是一个通用的框架，其性能很大程度上取决于其内部所使用的最小解法（Minimal Solver）。对于本质矩阵估计问题，理论上最少需要多少对点才能求解？答案是5对点：因为本质矩阵 \(\mathbf{E}\) 必须满足奇异值必须为 \((\sigma, \sigma, 0)\) 的的代数约束。这催生了由 David Nistér 提出的高效 5点算法，它利用5对归一化匹配点，通过求解一个10次多项式来恢复本质矩阵的所有可能解。
因此，RANSAC 算法和5点算法，正是在对极几何的工程实践中稳健求解本质矩阵的关键所在。
2 理论
2.1 5点算法
在上一节中，我们明确了5点算法作为RANSAC框架内最小解法的核心地位。现在，我们需要深入理解其本质：5点算法是一个代数求解过程，而非数值优化过程。这一区分至关重要，因为它决定了我们为何不能用Ceres等通用优化库来实现它。
与《最小二乘问题详解13：对极几何中本质矩阵求解》中介绍的8点算法（求解线性方程组）和Sampson优化（最小化非线性损失函数）不同，5点算法的目标是在恰好5对无噪声的归一化匹配点这一最小数据集上，完备地找出所有在几何上可能成立的本质矩阵。为了实现这一目标，算法必须严格遵守两个核心约束：
- 对极约束：对于每一对点 \((\tilde{\mathbf{x}}_{1i}, \tilde{\mathbf{x}}_{2i})\)，必须满足 \(\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i} = 0\)。
- 内在约束：本质矩阵 \(\mathbf{E}\) 的奇异值必须为 \((\sigma, \sigma, 0)\) 的形式。
将这两个约束条件结合起来，问题被转化为一个复杂的多项式方程组。David Nistér在其开创性工作中，通过精巧的代数操作（包括利用\(\mathbf{E}\)的内在约束将其参数化，并将对极约束代入），最终将这个系统简化为一个10次多项式方程。求解这个10次多项式的根，即可得到最多10个实数解。每一个实数解都对应一个候选的本质矩阵 \(\mathbf{E}\)。因此，5点算法的输出不是一个单一的解，而是一个包含最多10个候选模型的集合。
然而，这仅仅是第一步。由于多项式求解会引入数学上有效但物理上无效的解，我们必须对这些候选解进行后处理筛选。最常用的方法是Cheirality Check（深度一致性检查）：利用每个候选 \(\mathbf{E}\) 分解出对应的旋转 \(\mathbf{R}\) 和平移方向 \(\mathbf{t}\)，然后将空间点三角化，并检查这些点是否同时位于两个相机的前方（即深度为正）。只有通过此检查的解，才是物理世界中真实可行的解。
可以看到，整个5点算法流程——从建立多项式、求根到物理验证——是一个典型的符号计算与代数几何过程。它追求的是全局性和完备性，即不遗漏任何一个可能的正确答案。相比之下，以Ceres为代表的数值优化方法，其工作方式截然不同。它们需要一个初始猜测，并通过迭代的方式沿着梯度下降的方向去寻找一个局部最优解。这种方法在有良好初值和大量数据时非常高效和精确，但在仅有5个点且无可靠初值的情况下，极易陷入局部极小值或发散，无法保证找到全局正确的解。
鉴于5点算法的代数复杂性和实现难度（涉及高次多项式求根、病态方程处理等），并且考虑到本系列文章的核心主题是数值优化而非代数几何，我们在此不展开其繁琐的数学推导和代码实现细节。感兴趣的读者可以深入研读David Nistér的原始论文《An Efficient Solution to the Five-Point Relative Pose Problem》。在工程实践中，我们更倾向于直接使用经过充分测试和高度优化的开源库，如 OpenGV，它为我们提供了简洁、高效的5点算法接口，让我们能够专注于更高层次的系统构建与优化。
在 OpenGV 中调用5点算法的实现如下所示：- opengv::essentials_t estimateEssentialMatrix(
- const std::vector<Eigen::Vector3d>& points1,
- const std::vector<Eigen::Vector3d>& points2) {
- opengv::bearingVectors_t bv1, bv2;
- for (size_t i = 0; i < 5; ++i) {
- bv1.push_back(points1[i]);
- bv2.push_back(points2[i]);
- }
- opengv::relative_pose::CentralRelativeAdapter adapter(bv1, bv2);
- // 正确API
- opengv::essentials_t essential_matrices =
- opengv::relative_pose::fivept_nister(adapter);
- return essential_matrices;
- }
在真实世界中，我们面对的是一大堆混杂着大量错误匹配（外点）的数据。直接将5点算法应用于任意5个点，极大概率会因为选中外点而得到一个完全错误的模型。那么，如何才能从这堆“鱼龙混杂”的数据中，找到那个由真实内点所决定的正确模型呢？RANSAC（Random Sample Consensus，随机抽样一致性） 正是为解决此类问题而生的经典鲁棒估计算法。它的核心思想出奇地简单、优雅且强大。
2.2.1 基本直觉
想象你有一袋混合了红豆和绿豆的豆子，你的任务是找出红豆的大小。但问题是，你不知道哪些是红豆，哪些是绿豆，并且绿豆的数量可能远多于红豆。
RANSAC 的策略是：
- 随机抓一把：闭上眼睛，随机从袋子里抓出最少数量的豆子（比如5颗），假设它们都是红豆。
- 估算模型：根据这5颗“假设的红豆”来估算红豆的平均大小。
- 验证假设：拿着这个“红豆大小模型”，去检查袋子里所有的豆子。把那些大小与模型预测值接近的豆子（比如误差在1mm以内）都挑出来，这些就是本次假设下的“一致集（Consensus Set）”。
- 评估好坏：数一数这个“一致集”里有多少颗豆子。如果数量足够多（比如超过80%），那么我们就认为这次的假设很可能是对的，这个模型就是我们要找的红豆大小。
- 重复尝试：如果“一致集”太小，说明这次抓到的5颗豆子里很可能混入了绿豆，这次假设失败。没关系，我们把豆子放回去，重复以上过程很多次。
- 选择最佳：在所有尝试中，选择那个拥有最大一致集的模型作为最终结果。
在计算机视觉中，“豆子”就是特征匹配点对，“红豆”是内点，“绿豆”是外点，“红豆大小”就是我们要求解的几何模型（如本质矩阵 \(\mathbf{E}\)）。
2.2.2 标准流程
将上述直觉形式化，RANSAC求解本质矩阵的标准步骤如下：
- 确定参数：
- n：最小采样数。对于本质矩阵，n = 5。
- k：最大迭代次数。
- t：内点判定阈值。
- d：内点数量阈值。当一致集大小超过 d 时，即可提前终止并接受该模型。
- 循环 k 次或直到找到足够好的模型：
a. 随机采样：从总共 N 对匹配点中，均匀且随机地抽取 n=5 对点，构成一个假设的内点集。
b. 模型生成：使用这5对点，调用5点算法，计算出所有可能的本质矩阵候选解。
c. 模型验证：对于每一个候选解 \(\mathbf{E}_{candidate}\)：
- 遍历所有 N 对匹配点。
- 计算每对点 \((\tilde{\mathbf{x}}_1, \tilde{\mathbf{x}}_2)\) 到当前模型的几何距离（通常是 Sampson 距离或对极线距离）。
- 如果该距离小于阈值 t，则将此点对标记为该模型的内点。
- 统计该模型的内点总数。
d. 选择最佳候选：从5点算法返回的多个候选解中，选择内点数最多的那个作为本次迭代的最佳模型。
e. 更新全局最优：如果本次迭代的最佳模型拥有的内点数，比历史上所有迭代中找到的模型都多，则将其记录为当前全局最优模型。
- 最终优化：
- 使用全局最优模型所对应的内点集。
- 将这些干净的内点送入一个非线性优化器，对模型进行精细化调整，以获得最高精度的结果。
2.2.3 有效验证
RANSAC的有效性源于概率。假设数据集中内点的比例为 \(p\)（例如70%），那么一次随机抽取5个点全部是内点的概率为 \(p^5\)。虽然单次概率不高（\(0.7^5 \approx 0.168\)），但通过多次尝试（例如1000次），至少成功一次的概率会变得非常高（\(1 - (1-p^5)^k\)）。一旦有一次采样全是内点，5点算法就能恢复出正确的模型，而这个正确模型在验证阶段会吸引到几乎所有的内点，从而形成一个巨大的一致集，使其在竞争中胜出。
通过这种方式，RANSAC巧妙地绕开了对外点的直接处理，而是专注于寻找那个能被最多数据点支持的模型，从而实现了对高比例外点的强大鲁棒性。
3 实例
基于以上理论，具体的代码实现如下：
[code]#include #include #include #include #include #include #include #include #include #include using namespace std;using namespace Eigen;constexpr double PI = 3.14159265358979323846;using Vector9d = Eigen::Matrix;// ========================// 工具函数// ========================Eigen::Matrix3d Vec2Mat(const Vector9d& e) { return Eigen::Map(e.data());}Vector9d Mat2Vec(const Eigen::Matrix3d& E) { return Eigen::Map(E.data());}// Sampson 距离计算double SampsonDistance(const Eigen::Matrix3d& E, const Eigen::Vector3d& x1, const Eigen::Vector3d& x2) { double c = x2.transpose() * E * x1; Eigen::Vector3d Ex1 = E * x1; Eigen::Vector3d ETx2 = E.transpose() * x2; double denom = Ex1.squaredNorm() + ETx2.squaredNorm(); if (denom < 1e-12) return 1e10; return c * c / denom;}// 对极约束残差：x2^T E x1double EpipolarResidual(const Eigen::Matrix3d& E, const Eigen::Vector3d& x1, const Eigen::Vector3d& x2) { return x2.transpose() * E * x1;}// ========================// Ceres 残差块：Sampson 误差// ========================struct SampsonError { SampsonError(const Eigen::Vector3d& x1, const Eigen::Vector3d& x2) : x1_(x1), x2_(x2) {} template bool operator()(const T* const e_ptr, T* residual) const { Eigen::Map E(e_ptr); Eigen::Matrix x1_h(T(x1_(0)), T(x1_(1)), T(x1_(2))); Eigen::Matrix x2_h(T(x2_(0)), T(x2_(1)), T(x2_(2))); T c = x2_h.transpose() * E * x1_h; Eigen::Matrix Ex1 = E * x1_h; Eigen::Matrix ETx2 = E.transpose() * x2_h; T denom = Ex1.squaredNorm() + ETx2.squaredNorm(); if (denom < T(1e-12)) { residual[0] = T(1e5); } else { residual[0] = ceres::sqrt(c * c / denom); } return true; } static ceres::CostFunction* Create(const Eigen::Vector3d& x1, const Eigen::Vector3d& x2) { return new ceres::AutoDiffCostFunction( new SampsonError(x1, x2)); } private: Eigen::Vector3d x1_, x2_;};// ========================// 辅助函数：计算归一化变换矩阵 T// ========================Eigen::Matrix3d ComputeNormalization( const std::vector& points) { Eigen::Vector2d centroid(0, 0); for (const auto& p : points) { centroid += p.head(); } centroid /= points.size(); double avg_dist = 0.0; for (const auto& p : points) { avg_dist += (p.head() - centroid).norm(); } avg_dist /= points.size(); double scale = sqrt(2.0) / avg_dist; Eigen::Matrix3d T = Eigen::Matrix3d::Identity(); T(0, 0) = T(1, 1) = scale; T(0, 2) = -scale * centroid(0); T(1, 2) = -scale * centroid(1); return T;}// ========================// 8点算法实现// ========================Eigen::Matrix3d EightPointAlgorithm(const std::vector& x1s, const std::vector& x2s) { Eigen::Matrix3d T1 = ComputeNormalization(x1s); Eigen::Matrix3d T2 = ComputeNormalization(x2s); std::vector nx1s, nx2s; for (size_t i = 0; i < x1s.size(); ++i) { nx1s.push_back(T1 * x1s); nx2s.push_back(T2 * x2s); } size_t N = nx1s.size(); Eigen::MatrixXd A(N, 9); for (size_t i = 0; i < N; ++i) { double u1 = nx1s(0), v1 = nx1s(1); double u2 = nx2s(0), v2 = nx2s(1); A.row(i) |
提升卡
置顶卡
沉默卡
喧嚣卡
变色卡
千斤顶
照妖镜
