最小二乘问题详解14：鲁棒估计与5点算法求解本质矩阵
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;
}
2.2 RANSAC
在真实世界中，我们面对的是一大堆混杂着大量错误匹配（外点）的数据。直接将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 实例
基于以上理论，具体的代码实现如下：
#include <ceres/ceres.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
#include <random>
#include <vector>
using namespace std;
using namespace Eigen;
constexpr double PI = 3.14159265358979323846;
using Vector9d = Eigen::Matrix<double, 9, 1>;
// ========================
// 工具函数
// ========================
Eigen::Matrix3d Vec2Mat(const Vector9d& e) {
return Eigen::Map<const Eigen::Matrix3d>(e.data());
}
Vector9d Mat2Vec(const Eigen::Matrix3d& E) {
return Eigen::Map<const Vector9d>(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 x1
double 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 <typename T>
bool operator()(const T* const e_ptr, T* residual) const {
Eigen::Map<const Eigen::Matrix<T, 3, 3, Eigen::ColMajor>> E(e_ptr);
Eigen::Matrix<T, 3, 1> x1_h(T(x1_(0)), T(x1_(1)), T(x1_(2)));
Eigen::Matrix<T, 3, 1> x2_h(T(x2_(0)), T(x2_(1)), T(x2_(2)));
T c = x2_h.transpose() * E * x1_h;
Eigen::Matrix<T, 3, 1> Ex1 = E * x1_h;
Eigen::Matrix<T, 3, 1> 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<SampsonError, 1, 9>(
new SampsonError(x1, x2));
}
private:
Eigen::Vector3d x1_, x2_;
};
// ========================
// 辅助函数：计算归一化变换矩阵 T
// ========================
Eigen::Matrix3d ComputeNormalization(
const std::vector<Eigen::Vector3d>& points) {
Eigen::Vector2d centroid(0, 0);
for (const auto& p : points) {
centroid += p.head<2>();
}
centroid /= points.size();
double avg_dist = 0.0;
for (const auto& p : points) {
avg_dist += (p.head<2>() - 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<Eigen::Vector3d>& x1s,
const std::vector<Eigen::Vector3d>& x2s) {
Eigen::Matrix3d T1 = ComputeNormalization(x1s);
Eigen::Matrix3d T2 = ComputeNormalization(x2s);
std::vector<Eigen::Vector3d> nx1s, nx2s;
for (size_t i = 0; i < x1s.size(); ++i) {
nx1s.push_back(T1 * x1s[i]);
nx2s.push_back(T2 * x2s[i]);
}
size_t N = nx1s.size();
Eigen::MatrixXd A(N, 9);
for (size_t i = 0; i < N; ++i) {
double u1 = nx1s[i](0), v1 = nx1s[i](1);
double u2 = nx2s[i](0), v2 = nx2s[i](1);
A.row(i) << u1 * u2, v1 * u2, u2, u1 * v2, v1 * v2, v2, u1, v1, 1.0;
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullV);
Vector9d e = svd.matrixV().col(8);
Eigen::Matrix3d E_tilde = Vec2Mat(e);
Eigen::JacobiSVD<Eigen::Matrix3d> svd_E(
E_tilde, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector3d s = svd_E.singularValues();
double avg = (s(0) + s(1)) / 2.0;
Eigen::Matrix3d E_normalized =
svd_E.matrixU() * Eigen::DiagonalMatrix<double, 3, 3>(avg, avg, 0.0) *
svd_E.matrixV().transpose();
return T2.transpose() * E_normalized * T1;
}
// 归一化 E 使得 ||E||_F = 1
Eigen::Matrix3d NormalizeEssentialMatrix(const Eigen::Matrix3d& E) {
return E / E.norm();
}
//基于 Sampson 误差的非线性优化
Eigen::Matrix3d OptimizeSampsonError(Eigen::Matrix3d E_init,
const std::vector<Eigen::Vector3d>& x1s,
const std::vector<Eigen::Vector3d>& x2s) {
Vector9d e_opt = Mat2Vec(E_init);
ceres::Problem problem;
for (int i = 0; i < x1s.size(); ++i) {
problem.AddResidualBlock(SampsonError::Create(x1s[i], x2s[i]), nullptr,
e_opt.data());
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
options.max_num_iterations = 50;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << "\n" << summary.BriefReport() << "\n";
Eigen::Matrix3d E_opt = Vec2Mat(e_opt);
// 投影回本质矩阵流形（强制 σ1=σ2, σ3=0）
Eigen::JacobiSVD<Eigen::Matrix3d> svd(
E_opt, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector3d s = svd.singularValues();
double avg = (s(0) + s(1)) / 2.0;
Eigen::Matrix3d E_final = svd.matrixU() *
Eigen::DiagonalMatrix<double, 3, 3>(avg, avg, 0.0) *
svd.matrixV().transpose();
return E_final;
}
void GenerateData(int N_total, const Eigen::Matrix3d& K,
const Eigen::Matrix3d& R_gt, const Eigen::Vector3d& t_gt,
const Eigen::Matrix3d& E_gt,
std::vector<Eigen::Vector3d>& x1s,
std::vector<Eigen::Vector3d>& x1s_noisy,
std::vector<Eigen::Vector3d>& x2s,
std::vector<Eigen::Vector3d>& x2s_noisy) {
std::mt19937 gen(42);
// 内点数量：70%
int N_inliers = static_cast<int>(N_total * 0.7);
int N_outliers = N_total - N_inliers;
// === 1. 生成内点（真实投影 + 噪声）===
std::uniform_real_distribution<double> x_dist(-2.0, 2.0);
std::uniform_real_distribution<double> y_dist(-2.0, 2.0);
std::uniform_real_distribution<double> z_dist(4.0, 8.0);
std::normal_distribution<double> noise(0.0, 1.0); // 像素噪声
for (int i = 0; i < N_inliers; ++i) {
Eigen::Vector3d Pw(x_dist(gen), y_dist(gen), z_dist(gen));
// 投影到相机1
Eigen::Vector3d Pc1 = Pw;
double u1 = K(0, 0) * Pc1(0) / Pc1(2) + K(0, 2);
double v1 = K(1, 1) * Pc1(1) / Pc1(2) + K(1, 2);
// 投影到相机2
Eigen::Vector3d Pc2 = R_gt * Pw + t_gt;
double u2 = K(0, 0) * Pc2(0) / Pc2(2) + K(0, 2);
double v2 = K(1, 1) * Pc2(1) / Pc2(2) + K(1, 2);
// 加像素噪声
double u1_noisy = u1 + noise(gen);
double v1_noisy = v1 + noise(gen);
double u2_noisy = u2 + noise(gen);
double v2_noisy = v2 + noise(gen);
// 转为归一化坐标
Eigen::Vector3d pixel1(u1, v1, 1);
Eigen::Vector3d pixel2(u2, v2, 1);
Eigen::Vector3d pixel1_noisy(u1_noisy, v1_noisy, 1);
Eigen::Vector3d pixel2_noisy(u2_noisy, v2_noisy, 1);
Eigen::Vector3d x1 = (K.inverse() * pixel1).normalized();
Eigen::Vector3d x2 = (K.inverse() * pixel2).normalized();
Eigen::Vector3d x1_noisy = (K.inverse() * pixel1_noisy).normalized();
Eigen::Vector3d x2_noisy = (K.inverse() * pixel2_noisy).normalized();
x1s.push_back(x1);
x2s.push_back(x2);
x1s_noisy.push_back(x1_noisy);
x2s_noisy.push_back(x2_noisy);
}
// === 2. 生成外点（完全随机方向）===
for (int i = 0; i < N_outliers; ++i) { // 方法：从单位球面前半球随机采样
Eigen::Vector3d dir1, dir2;
do {
dir1 = Eigen::Vector3d::Random();
} while (dir1.z() <= 0 || dir1.norm() < 1e-6);
do {
dir2 = Eigen::Vector3d::Random();
} while (dir2.z() <= 0 || dir2.norm() < 1e-6);
dir1.normalize();
dir2.normalize();
x1s.push_back(dir1);
x2s.push_back(dir2);
x1s_noisy.push_back(dir1);
x2s_noisy.push_back(dir2);
}
// === 3. 打乱数据顺序（保持对应关系）===
std::vector<size_t> indices(N_total);
std::iota(indices.begin(), indices.end(), 0);
std::shuffle(indices.begin(), indices.end(), gen);
auto x1s_old = x1s;
auto x1s_noisy_old = x1s_noisy;
auto x2s_old = x2s;
auto x2s_noisy_old = x2s_noisy;
for (size_t i = 0; i < N_total; ++i) {
x1s[i] = x1s_old[indices[i]];
x1s_noisy[i] = x1s_noisy_old[indices[i]];
x2s[i] = x2s_old[indices[i]];
x2s_noisy[i] = x2s_noisy_old[indices[i]];
}
std::cout << "Generated " << N_total << " correspondences: " << N_inliers
<< " inliers + " << N_outliers << " outliers ("
<< (100.0 * N_outliers / N_total) << "% outliers)\n\n";
}
// RANSAC + 5-point (OpenGV)
// 返回：Essential Matrix + inlier mask
std::pair<Eigen::Matrix3d, std::vector<bool>> EstimateEssentialMatrixRansac(
const std::vector<Eigen::Vector3d>& x1s,
const std::vector<Eigen::Vector3d>& x2s, double pixel_threshold = 1.0,
double focal_length = 500.0, int max_iterations = 1000) {
size_t N = x1s.size();
if (x1s.size() != x2s.size() || x1s.size() < 5) {
std::cerr << "Not enough correspondences.\n";
return {Eigen::Matrix3d::Identity(), std::vector<bool>(N, false)};
}
// OpenGV expects unit bearing vectors
opengv::bearingVectors_t b1(x1s.begin(), x1s.end());
opengv::bearingVectors_t b2(x2s.begin(), x2s.end());
opengv::relative_pose::CentralRelativeAdapter adapter(b1, b2);
//中心相机模型下的相对位姿 SAC 问题
using SacProblem =
opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem;
auto problem = std::make_shared<SacProblem>(adapter, SacProblem::NISTER);
opengv::sac::Ransac<SacProblem> ransac;
ransac.sac_model_ = problem;
// 将像素级内点阈值（如1.0像素）转换为OpenGV所需的弦距离阈值
// 步骤：pixel → 近似视角θ ≈ pixel/f → 弦距离 = 2*sin(θ/2)
double theta_approx = pixel_threshold / focal_length; // 小角度近似: θ ≈ tanθ = pixel/f
ransac.threshold_ = 2.0 * sin(theta_approx / 2.0); // 转换为单位球面上的弦距离
ransac.max_iterations_ = max_iterations;
if (!ransac.computeModel()) {
std::cerr << "RANSAC failed.\n";
return {Eigen::Matrix3d::Identity(), std::vector<bool>(N, false)};
}
// OpenGV 返回的是 3x4 pose matrix [R | t]
Eigen::Matrix<double, 3, 4> Rt = ransac.model_coefficients_;
Eigen::Matrix3d R = Rt.leftCols<3>();
Eigen::Vector3d t = Rt.rightCols<1>();
// 构造 skew-symmetric matrix
Eigen::Matrix3d t_hat;
t_hat << 0, -t.z(), t.y(), t.z(), 0, -t.x(), -t.y(), t.x(), 0;
Eigen::Matrix3d E = t_hat * R;
// inlier mask
std::vector<bool> inliers(N, false);
for (int idx : ransac.inliers_) {
if (idx >= 0 && idx < static_cast<int>(N)) inliers[idx] = true;
}
std::cout << "RANSAC inliers: " << ransac.inliers_.size() << " / " << N
<< std::endl;
return {E, inliers};
}
// ========================
// 主函数（仅评估 E）
// ========================
int main() {
std::cout << std::fixed << std::setprecision(6);
// === 1. Ground Truth Essential Matrix ===
Eigen::Matrix3d K; // 相机内参矩阵 K 的定义
K << 500, 0, 320, 0, 500, 240, 0, 0, 1;
Eigen::Matrix3d R_gt =
Eigen::AngleAxisd(PI / 6.0, Eigen::Vector3d::UnitY()).toRotationMatrix();
Eigen::Vector3d t_gt(0.8, -0.2, 0.5);
Eigen::Matrix3d t_skew;
t_skew << 0, -t_gt(2), t_gt(1), t_gt(2), 0, -t_gt(0), -t_gt(1), t_gt(0), 0;
Eigen::Matrix3d E_gt = t_skew * R_gt;
std::cout << "=== Ground Truth Essential Matrix ===\n";
std::cout << E_gt << "\n\n";
// === 2. 生成带噪声的归一化图像点 ===
int N = 120;
std::vector<Eigen::Vector3d> x1s, x1s_noisy;
std::vector<Eigen::Vector3d> x2s, x2s_noisy;
GenerateData(N, K, R_gt, t_gt, E_gt, x1s, x1s_noisy, x2s, x2s_noisy);
// === 3. RANSAC + 5点算法 ===
auto [E_ransac, inlier_mask] =
EstimateEssentialMatrixRansac(x1s_noisy, x2s_noisy, 1.0, 500, 1000);
// 提取内点
std::vector<Eigen::Vector3d> x1_inliers, x2_inliers;
for (size_t i = 0; i < x1s_noisy.size(); ++i) {
if (inlier_mask[i]) {
x1_inliers.push_back(x1s_noisy[i]);
x2_inliers.push_back(x2s_noisy[i]);
}
}
x1s_noisy.swap(x1_inliers);
x2s_noisy.swap(x2_inliers);
N = (int)x1s_noisy.size();
// === 4. 初值：8点算法 ===
Eigen::Matrix3d E_8pt = EightPointAlgorithm(x1s_noisy, x2s_noisy);
std::cout << "=== 8-Point Estimate ===\n";
std::cout << E_8pt << "\n\n";
// === 5. 非线性优化：Sampson 误差 ===
Eigen::Matrix3d E_final = OptimizeSampsonError(E_8pt, x1s_noisy, x2s_noisy);
std::cout << "=== Final Optimized E (Projected) ===\n";
std::cout << E_final << "\n\n";
// === 6. 评估三项指标 ===
double sampson_8pt = 0.0, sampson_final = 0.0;
double epipolar_mse_8pt = 0.0, epipolar_mse_final = 0.0;
for (int i = 0; i < N; ++i) {
sampson_8pt += SampsonDistance(E_8pt, x1s_noisy[i], x2s_noisy[i]);
sampson_final += SampsonDistance(E_final, x1s_noisy[i], x2s_noisy[i]);
double r1 = EpipolarResidual(E_8pt, x1s_noisy[i], x2s_noisy[i]);
double r2 = EpipolarResidual(E_final, x1s_noisy[i], x2s_noisy[i]);
epipolar_mse_8pt += r1 * r1;
epipolar_mse_final += r2 * r2;
}
sampson_8pt /= N;
sampson_final /= N;
epipolar_mse_8pt /= N;
epipolar_mse_final /= N;
// 归一化 Frobenius 距离（代数相似性）
Eigen::Matrix3d E_gt_norm = NormalizeEssentialMatrix(E_gt);
Eigen::Matrix3d E_8pt_norm = NormalizeEssentialMatrix(E_8pt);
Eigen::Matrix3d E_final_norm = NormalizeEssentialMatrix(E_final);
double frob_8pt = (E_8pt_norm - E_gt_norm).norm();
double frob_final = (E_final_norm - E_gt_norm).norm();
// === 6. 输出评估结果 ===
std::cout << "=== Evaluation Summary ===\n";
std::cout << "Metric | 8-Point | Optimized\n";
std::cout << "---------------------------|---------------|------------\n";
std::cout << "Avg Sampson Error | " << setw(13) << sampson_8pt
<< " | " << sampson_final << "\n";
std::cout << "Avg |x2^T E x1|^2 (MSE) | " << setw(13) << epipolar_mse_8pt
<< " | " << epipolar_mse_final << "\n";
std::cout << "Normalized Frobenius Dist | " << setw(13) << frob_8pt << " | "
<< frob_final << "\n\n";
// === 7. 结论性说明 ===
std::cout << "Interpretation:\n";
std::cout << "- Lower Sampson error and epipolar MSE indicate better "
"geometric consistency.\n";
std::cout << "- Smaller normalized Frobenius distance suggests closer "
"algebraic alignment with ground truth.\n";
std::cout << "- In practice, geometric metrics (Sampson, epipolar residual) "
"are more reliable than Frobenius distance.\n";
return 0;
}
整个流程的关键在于 EstimateEssentialMatrixRansac 函数的实现。OpenGV 巧妙地封装了 RANSAC 框架及其内部的 5 点最小解法，使得我们能够以极简的接口完成鲁棒的本质矩阵估计。该函数的核心步骤包括：
- 数据准备：将输入的归一化匹配点转换为 OpenGV 所需的
bearingVectors_t格式。 - 适配器构建：创建
CentralRelativeAdapter，用于管理两视图的观测数据。 - SAC 问题定义：指定使用
CentralRelativePoseSacProblem并选择NISTER（即5点）算法作为最小解法。 - RANSAC 配置与执行：设置内点判定阈值（将像素误差转换为归一化空间的角度误差）和最大迭代次数，然后调用
computeModel()执行完整的 RANSAC 流程。 - 结果后处理：从 OpenGV 返回的
[R|t]姿态矩阵中重构出本质矩阵 \(\mathbf{E}\)，并生成内点掩码。
关于阈值的几何意义：
OpenGV 在判断一个点是否为内点时，并非直接使用角度或像素误差，而是采用两个单位方向向量（即归一化坐标）之间的弦距离（chordal distance），其定义为 \(\|\mathbf{b}_1 - \mathbf{b}_2\| = 2 \sin(\theta/2)\)，其中 \(\theta\) 是两向量间的夹角。
用户通常以像素误差（如 1 像素）的形式指定容差。在针孔模型中，该误差对应视角偏差满足 \(\tan\theta = \text{pixel}/f\)。由于实际应用中 \(\text{pixel}/f \ll 1\)（例如 \(1/500\)），可安全使用小角度近似 \(\theta \approx \text{pixel}/f\)。
于是，对应的弦距离阈值即为 \(2 \sin(\theta/2) \approx 2 \sin\big((\text{pixel}/f)/2\big)\)。代码中angle = pixel_threshold / focal_length正是这一近似的体现，而2 * sin(angle / 2)则完成了从像素误差到 OpenGV 所需弦距离阈值的精确几何转换。这种处理既保持了计算效率，又确保了内点判定的几何一致性。
另一个关键环节在于，EstimateEssentialMatrixRansac 成功识别出内点后，我们将这些“清洗”过的内点传递给经典的 8点算法 以获得一个良好的初值，继而利用 Ceres 进行基于 Sampson 误差的非线性优化，从而在几何上精炼最终的模型。
程序运行后，输出结果如下所示：
=== Ground Truth Essential Matrix ===
0.100000 -0.500000 -0.173205
0.833013 0.000000 -0.442820
0.173205 0.800000 0.100000
Generated 120 correspondences: 84 inliers + 36 outliers (30.000000% outliers)
RANSAC inliers: 86 / 120
=== 8-Point Estimate ===
0.141962 4.320688 0.875926
-4.116816 0.914296 -4.538002
-0.780423 1.559083 -0.631194
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 9.841825e-02 0.00e+00 1.99e-01 0.00e+00 0.00e+00 1.00e+04 0 1.53e-04 3.04e-04
1 1.817917e-02 8.02e-02 3.08e-02 0.00e+00 9.89e-01 3.00e+04 1 1.55e-04 5.60e-04
2 3.119957e-02 -1.30e-02 3.08e-02 8.50e+00 -1.18e+00 1.50e+04 1 5.10e-05 7.54e-04
3 3.057134e-02 -1.24e-02 3.08e-02 8.16e+00 -1.12e+00 3.75e+03 1 1.96e-05 8.80e-04
4 2.226714e-02 -4.09e-03 3.08e-02 6.80e+00 -3.77e-01 4.69e+02 1 2.52e-05 9.99e-04
5 7.969384e-03 1.02e-02 9.53e-03 4.23e+00 1.06e+00 1.41e+03 1 1.73e-04 1.45e-03
6 4.996285e-03 2.97e-03 1.75e-02 5.00e+00 7.21e-01 1.54e+03 1 1.18e-04 1.83e-03
7 2.694957e-03 2.30e-03 6.79e-03 1.92e+00 9.85e-01 4.62e+03 1 1.20e-04 2.23e-03
8 2.451344e-03 2.44e-04 1.73e-04 3.99e-01 1.05e+00 1.39e+04 1 1.51e-04 2.67e-03
9 2.447292e-03 4.05e-06 1.46e-05 1.20e-01 1.28e+00 4.16e+04 1 1.15e-04 3.05e-03
10 2.446955e-03 3.36e-07 4.12e-06 3.49e-02 1.29e+00 1.25e+05 1 1.02e-04 3.33e-03
11 2.446927e-03 2.86e-08 1.22e-06 1.02e-02 1.29e+00 3.74e+05 1 1.06e-04 3.58e-03
Ceres Solver Report: Iterations: 12, Initial cost: 9.841825e-02, Final cost: 2.446927e-03, Termination: CONVERGENCE
=== Final Optimized E (Projected) ===
-1.590565 1.745187 2.234801
-0.628001 0.065600 -2.317624
-2.207515 1.526189 -1.496869
=== Evaluation Summary ===
Metric | 8-Point | Optimized
---------------------------|---------------|------------
Avg Sampson Error | 0.002289 | 0.000518
Avg |x2^T E x1|^2 (MSE) | 0.072808 | 0.010068
Normalized Frobenius Dist | 1.581131 | 1.437171
Interpretation:
- Lower Sampson error and epipolar MSE indicate better geometric consistency.
- Smaller normalized Frobenius distance suggests closer algebraic alignment with ground truth.
- In practice, geometric metrics (Sampson, epipolar residual) are more reliable than Frobenius distance.
从这个输出结果可以看到：
- RANSAC 表现：在包含 30% 外点的数据集中，RANSAC 成功识别出 86 个内点，非常接近真实的 84 个内点，证明了其强大的鲁棒性。多出的 2 个点很可能是外点中恰好满足所估计本质矩阵对极约束的“伪内点”（pseudo-inliers），这在存在噪声和有限阈值的情况下是正常现象。
- 优化效果：8点算法的结果虽然代数上满足约束，但其几何误差（Sampson Error 和 Epipolar MSE）相对较高。经过 Ceres 的非线性优化后，两项几何误差指标均显著下降（Sampson 误差从
0.002289降至0.000518），表明模型的几何一致性得到了极大提升。 - 指标对比：值得注意的是，尽管优化后的模型在几何上更优，但其与真值的归一化 Frobenius 距离并未成为最小。这是因为 Frobenius 距离是一个代数指标，不能完全反映几何精度；在评估对极几何模型时，应优先信赖 Sampson 误差等几何度量。
浙公网安备 33010602011771号