最小二乘问题详解13：对极几何中本质矩阵求解

1 引言

在本系列（《最小二乘问题详解：目录》）的前三篇文章中，我们系统探讨了运动恢复结构（Structure from Motion, SFM）中的两个核心子问题：

• PnP 问题（《最小二乘问题详解10：PnP问题求解》与《最小二乘问题详解11：基于李代数的PnP优化》）：在已知部分 3D 结构的前提下，通过 2D-3D 对应关系求解相机位姿；
  
• 三角化（《最小二乘问题详解12：三角化中的非线性优化》）：在已知相机位姿的前提下，通过多视角 2D 观测反推空间点的 3D 位置。
  

这两个问题互为对偶，共同构成了 SFM 系统的“闭环”：PnP 扩展位姿，三角化扩展结构。然而，它们都依赖一个关键前提——系统中必须已有初始的 3D 点或相机位姿。
那么，当面对一组全新的图像序列，既无任何 3D 点云，也无任何相机位姿时，我们该如何迈出第一步？答案正是本文的主题：对极几何（Epipolar Geometry）。对极几何是 SFM 初始化阶段的基础。它允许我们仅凭两幅图像中的 2D-2D 特征匹配，在不依赖任何先验 3D 信息的情况下，估计出两个相机之间的相对运动（旋转 \(\mathbf{R}\) 与平移方向 \(\mathbf{t}\)）。这一估计结果，将作为后续三角化生成初始 3D 点的依据，从而正式启动整个 SFM 流程。
然而，对极几何的完整理论涵盖多种场景，其复杂性不容小觑。为了清晰阐述并聚焦工程实践，本文将做出一个关键且合理的假设：相机已经过标定，内参矩阵 \(\mathbf{K}\) 已知且恒定。这一假设在绝大多数实际应用中（如机器人导航、增强现实、无人机测绘等）都成立。在此前提下，我们可以直接估计本质矩阵（Essential Matrix, \(\mathbf{E}\)），进而高效、鲁棒地恢复相机的相对位姿，并为后续的非线性优化（如束调整）奠定坚实基础。在下篇中，我们将探讨更具挑战性的未知内参场景。
本文将遵循本系列一贯的风格：从几何模型出发，建立最小二乘优化问题，分析其线性与非线性解法，并最终通过实例代码验证理论。
2 几何模型

2.1 从成像模型出发

对极几何的推导始于我们熟悉的针孔相机成像模型。考虑两个相机观测同一静态场景，其投影过程可分别描述为：

\[\lambda_1 \mathbf{x}_1 = \mathbf{K}_1 [\mathbf{R}_1 \mid \mathbf{t}_1] \tilde{\mathbf{X}}, \quad\lambda_2 \mathbf{x}_2 = \mathbf{K}_2 [\mathbf{R}_2 \mid \mathbf{t}_2] \tilde{\mathbf{X}}\tag{1}\]
其中：

• \(\tilde{\mathbf{X}} = [\mathbf{X}^\top, 1]^\top \in \mathbb{R}^4\) 是空间点 \(\mathbf{X} \in \mathbb{R}^3\) 的齐次坐标；
  
• \(\mathbf{x}_1, \mathbf{x}_2 \in \mathbb{R}^3\) 是其在两幅图像中的齐次像素坐标；
  
• \(\mathbf{K}_1, \mathbf{K}_2\) 为相机内参矩阵；
  
• \([\mathbf{R}_i \mid \mathbf{t}_i]\) 为第 \(i\) 个相机的外参（从世界坐标系到相机坐标系的变换）；
  
• \(\lambda_1, \lambda_2\) 为未知的正尺度因子（即深度）。
  

为简化分析，我们将世界坐标系固定在第一个相机上。这是一个无损一般性的选择，因为相对运动与坐标系选取无关。于是有：

\[\mathbf{R}_1 = \mathbf{I}, \quad \mathbf{t}_1 = \mathbf{0}\quad \Rightarrow \quad\mathbf{P}_1 = \mathbf{K}_1 [\mathbf{I} \mid \mathbf{0}]\]
令第二个相机相对于第一个的位姿为 \((\mathbf{R}, \mathbf{t})\)，即：

\[\mathbf{R}_2 = \mathbf{R}, \quad \mathbf{t}_2 = \mathbf{t}\quad \Rightarrow \quad\mathbf{P}_2 = \mathbf{K}_2 [\mathbf{R} \mid \mathbf{t}]\]
此时，式 (1) 简化为：

\[\lambda_1 \mathbf{x}_1 = \mathbf{K}_1 \mathbf{X}, \quad\lambda_2 \mathbf{x}_2 = \mathbf{K}_2 (\mathbf{R} \mathbf{X} + \mathbf{t})\tag{2}\]
2.2 归一化图像坐标

由于本文假设相机已标定，内参矩阵 \(\mathbf{K}_1\) 和 \(\mathbf{K}_2\) 已知。我们可以将像素坐标“反投影”回各自相机坐标系下的方向向量，得到归一化图像坐标（normalized image coordinates）：

\[\tilde{\mathbf{x}}_1 = \mathbf{K}_1^{-1} \mathbf{x}_1, \quad\tilde{\mathbf{x}}_2 = \mathbf{K}_2^{-1} \mathbf{x}_2\tag{3}\]
这些向量位于相机坐标系的归一化成像平面（即 \(Z=1\) 平面）上，其物理意义是从相机光心出发、指向对应像素的视线方向。
为了建立两视图间的几何关系，我们对式 (2) 中的第一式左乘 \(\mathbf{K}_1^{-1}\)，得：

\[\mathbf{X} = \lambda_1 \mathbf{K}_1^{-1} \mathbf{x}_1 = \lambda_1 \tilde{\mathbf{x}}_1\tag{4}\]
这表明 \(\mathbf{X}\) 位于方向 \(\tilde{\mathbf{x}}_1\) 的射线上，距离由 \(\lambda_1\) 决定。接下来将 (4) 代入 (2) 中第二式，并左乘 \(\mathbf{K}_2^{-1}\)，有：

\[\lambda_2 \tilde{\mathbf{x}}_2 = \mathbf{R} (\lambda_1 \tilde{\mathbf{x}}_1) + \mathbf{t}\]
整理后得到：

\[\lambda_2 \tilde{\mathbf{x}}_2 - \lambda_1 \mathbf{R} \tilde{\mathbf{x}}_1 = \mathbf{t}\tag{5}\]
式 (5) 揭示了一个关键几何事实：平移向量 \(\mathbf{t}\) 可表示为 \(\tilde{\mathbf{x}}_2\) 与 \(\mathbf{R} \tilde{\mathbf{x}}_1\) 的线性组合，因此三个向量 \(\mathbf{t}\)、\(\mathbf{R} \tilde{\mathbf{x}}_1\) 和 \(\tilde{\mathbf{x}}_2\) 必然共面。这一共面性正是对极约束的几何根源。
2.3 引入叉积与本质矩阵

三个向量共面的充要条件是它们的混合积为零，即：

\[\tilde{\mathbf{x}}_2^\top \left( \mathbf{t} \times (\mathbf{R} \tilde{\mathbf{x}}_1) \right) = 0\tag{6}\]
利用叉积的矩阵表示 \(\mathbf{a} \times \mathbf{b} = [\mathbf{a}]_\times \mathbf{b}\)，其中 \([\mathbf{a}]_\times\) 是反对称矩阵：

\[[\mathbf{a}]_\times =\begin{bmatrix}0 & -a_z & a_y \\a_z & 0 & -a_x \\-a_y & a_x & 0\end{bmatrix}\]
式 (6) 可重写为：

\[\tilde{\mathbf{x}}_2^\top [\mathbf{t}]_\times \mathbf{R} \tilde{\mathbf{x}}_1 = 0\tag{7}\]
我们定义 本质矩阵（Essential Matrix）为：

\[\boxed{\mathbf{E} = [\mathbf{t}]_\times \mathbf{R}}\tag{8}\]
于是，最终得到对极约束（Epipolar Constraint）：

\[\boxed{\tilde{\mathbf{x}}_2^\top \mathbf{E} \tilde{\mathbf{x}}_1 = 0}\tag{9}\]
2.4 几何意义总结

从以上推导我们可以得出以下几点：

• 归一化坐标 \(\tilde{\mathbf{x}}\) 消除了相机内参的影响，使问题聚焦于纯粹的几何关系；
  
• 本质矩阵 \(\mathbf{E}\) 完全由两个相机之间的相对旋转 \(\mathbf{R}\) 和平移 \(\mathbf{t}\) 决定；
  
• 对极约束 表明：给定第一幅图像中的点 \(\tilde{\mathbf{x}}_1\)，其在第二幅图像中的对应点 \(\tilde{\mathbf{x}}_2\) 必定位于一条直线上——即对极线 \(\ell_2 = \mathbf{E} \tilde{\mathbf{x}}_1\)。
  

这一约束构成了后续所有优化与鲁棒估计的基础。
3 问题建模

有了对极约束 \(\tilde{\mathbf{x}}_2^\top \mathbf{E} \tilde{\mathbf{x}}_1 = 0\)，我们的问题转化为：给定 \(N\) 对归一化匹配点 \(\{(\tilde{\mathbf{x}}_{1i}, \tilde{\mathbf{x}}_{2i})\}_{i=1}^N\)，如何估计本质矩阵 \(\mathbf{E}\)？这是一个典型的齐次线性最小二乘问题，但其解需满足本质矩阵的内在几何约束。下面我们将从线性初值估计出发，逐步过渡到更鲁棒、更精确的非线性优化框架。
3.1 线性求解：8点算法

将本质矩阵 \(\mathbf{E}\) 按列堆叠为9维向量 \(\mathbf{e} = \mathrm{vec}(\mathbf{E})\)。对每一对归一化匹配点 \((\tilde{\mathbf{x}}_{1i}, \tilde{\mathbf{x}}_{2i})\)，其中 \(\tilde{\mathbf{x}}_{1i} = [\tilde{u}_{1i}, \tilde{v}_{1i}, 1]^\top\)、\(\tilde{\mathbf{x}}_{2i} = [\tilde{u}_{2i}, \tilde{v}_{2i}, 1]^\top\)，对极约束 \(\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i} = 0\) 可展开为一个线性方程。
具体地，设

\[\mathbf{E} = \begin{bmatrix}e_{11} & e_{12} & e_{13} \\e_{21} & e_{22} & e_{23} \\e_{31} & e_{32} & e_{33}\end{bmatrix},\quad\mathbf{e} = \mathrm{vec}(\mathbf{E}) = \begin{bmatrix}e_{11} \\ e_{21} \\ e_{31} \\e_{12} \\ e_{22} \\ e_{32} \\e_{13} \\ e_{23} \\ e_{33}\end{bmatrix}\]
则通过显式计算 \(\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i}\) 并按 \(\mathbf{e}\) 的顺序整理系数，可得：

\[\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i} = \begin{bmatrix}\tilde{u}_{1i}\tilde{u}_{2i} &\tilde{u}_{1i}\tilde{v}_{2i} &\tilde{u}_{1i} &\tilde{v}_{1i}\tilde{u}_{2i} &\tilde{v}_{1i}\tilde{v}_{2i} &\tilde{v}_{1i} &\tilde{u}_{2i} &\tilde{v}_{2i} &1\end{bmatrix}\mathbf{e} = 0\]
将 \(N\) 个这样的约束堆叠，得到齐次线性系统：

\[\tilde{\mathbf{A}} \mathbf{e} = \mathbf{0}, \quad \tilde{\mathbf{A}} \in \mathbb{R}^{N \times 9}\tag{10}\]
如同三角化问题求解一样（参见《最小二乘问题详解12：三角化中的非线性优化》），这种齐次线性方程组适合 SVD 方法求非零解，并且其解 \(\mathbf{e}\) 为 \(\tilde{\mathbf{A}}\) 最小奇异值对应的右奇异向量。
然而，由此得到的 \(\mathbf{E}_{\text{linear}}\) 通常不满足本质矩阵的几何约束。这是因为本质矩阵并非任意 \(3\times3\) 矩阵，而是由相对位姿 \((\mathbf{R}, \mathbf{t})\) 构造而来，即\(\mathbf{E} = [\mathbf{t}]_\times \mathbf{R}\) 。其中 \([\mathbf{t}]_\times\) 是秩为2的反对称矩阵，\(\mathbf{R}\) 是正交矩阵（\(\det(\mathbf{R})=1\)）。由此可推导出 \(\mathbf{E}\) 必须满足两个关键性质（参见《计算机视觉中的多视图几何》）：

• 秩为2：\(\mathrm{rank}(\mathbf{E}) = 2\)；
  
• 奇异值结构：\(\mathbf{E}\) 的两个非零奇异值相等，即其奇异值形如 \((\sigma, \sigma, 0)\)。
  

这两个性质是 \(\mathbf{E}\) 能分解为合法 \((\mathbf{R}, \mathbf{t})\) 的充要条件。若直接使用线性解 \(\mathbf{E}_{\text{linear}}\)，其秩通常为3，奇异值也不相等，导致后续无法正确恢复位姿。因此，必须进行后处理强制约束：对 \(\mathbf{E}_{\text{linear}}\) 做 SVD，

\[\mathbf{E}_{\text{linear}} = \mathbf{U} \, \mathrm{diag}(\sigma_1, \sigma_2, \sigma_3) \, \mathbf{V}^\top, \quad \sigma_1 \geq \sigma_2 \geq \sigma_3 \geq 0\]
然后令 \(\sigma_3 = 0\)，并将前两个奇异值设为它们的平均值以满足相等性：

\[\mathbf{E}_{\text{8pt}} = \mathbf{U} \, \mathrm{diag}\left( \frac{\sigma_1 + \sigma_2}{2}, \frac{\sigma_1 + \sigma_2}{2}, 0 \right) \, \mathbf{V}^\top\]
这一操作确保了 \(\mathbf{E}_{\text{8pt}}\) 满足 \(\mathrm{rank}=2\) 且奇异值相等，从而可以被唯一分解为有效的旋转和平移方向（最多四组解，通过 cheirality check 筛选）。
这就是经典的8点算法（需 \(N \geq 8\)）。但该方法与三角化问题得线性求解法一样，最小化的是代数误差 \(\|\tilde{\mathbf{A}} \mathbf{e}\|^2\)，其物理意义不明确，对噪声敏感，通常仅用作初值，后续还需进行非线性优化。
3.2 非线性优化：Sampson 误差

为获得更具几何意义的解，我们希望最小化重投影误差。然而，在仅有2D观测、无3D点的情况下，无法直接构建重投影误差（因为缺少3D点作为中间变量）。此时，Sampson 误差提供了一个优良的一阶近似。
给定当前估计的本质矩阵 \(\mathbf{E}\)，一对匹配点 \((\tilde{\mathbf{x}}_{1i}, \tilde{\mathbf{x}}_{2i})\) 通常不严格满足对极约束，即其约束违反量为：

\[c_i = \tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i} \neq 0\]
我们假设这是由于图像噪声导致的观测偏差，并设想存在一个“真实”的无噪声点对 \((\tilde{\mathbf{x}}_{1i} + \delta_1, \tilde{\mathbf{x}}_{2i} + \delta_2)\)，它精确满足对极约束：

\[(\tilde{\mathbf{x}}_{2i} + \delta_2)^\top \mathbf{E} (\tilde{\mathbf{x}}_{1i} + \delta_1) = 0\tag{12}\]
我们的目标是找到最小的图像扰动 \((\delta_1, \delta_2)\) 使得上式成立，并以总扰动能量

\[\|\delta_1\|^2 + \|\delta_2\|^2\]
作为该点对的误差度量。这就是 Sampson 误差的核心思想。
由于 \(\delta_1, \delta_2\) 很小（小噪声假设），我们可以对式 (12) 进行的左边直接乘开，忽略二阶小量 \(\delta_2^\top \mathbf{E} \delta_1\)，得到：

\[\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i} + \delta_2^\top \mathbf{E} \tilde{\mathbf{x}}_{1i} + \tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \delta_1 \approx 0\]
注意到 \(\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \delta_1 = (\mathbf{E}^\top \tilde{\mathbf{x}}_{2i})^\top \delta_1\)，因此约束可写为：

\[(\mathbf{E} \tilde{\mathbf{x}}_{1i})^\top \delta_2 + (\mathbf{E}^\top \tilde{\mathbf{x}}_{2i})^\top \delta_1 = -c_i\tag{13}\]
现在问题转化为：在满足线性约束 (13) 的前提下，最小化二次目标函数 \(\|\delta_1\|^2 + \|\delta_2\|^2\)。这是一个标准的带等式约束的最小二乘问题，可用拉格朗日乘子法求解。
构造拉格朗日函数：

\[\mathcal{L} = \|\delta_1\|^2 + \|\delta_2\|^2 + 2\lambda \left[ (\mathbf{E} \tilde{\mathbf{x}}_{1i})^\top \delta_2 + (\mathbf{E}^\top \tilde{\mathbf{x}}_{2i})^\top \delta_1 + c_i \right]\]
对 \(\delta_1, \delta_2\) 求导并令导数为零：

\[\frac{\partial \mathcal{L}}{\partial \delta_1} = 2\delta_1 + 2\lambda \mathbf{E}^\top \tilde{\mathbf{x}}_{2i} = 0 \quad \Rightarrow \quad \delta_1 = -\lambda \mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\]

\[\frac{\partial \mathcal{L}}{\partial \delta_2} = 2\delta_2 + 2\lambda \mathbf{E} \tilde{\mathbf{x}}_{1i} = 0 \quad \Rightarrow \quad \delta_2 = -\lambda \mathbf{E} \tilde{\mathbf{x}}_{1i}\]
将 \(\delta_1, \delta_2\) 代入约束 (13)：

\[(\mathbf{E} \tilde{\mathbf{x}}_{1i})^\top (-\lambda \mathbf{E} \tilde{\mathbf{x}}_{1i}) + (\mathbf{E}^\top \tilde{\mathbf{x}}_{2i})^\top (-\lambda \mathbf{E}^\top \tilde{\mathbf{x}}_{2i}) = -c_i\]
即：

\[-\lambda \left( \|\mathbf{E} \tilde{\mathbf{x}}_{1i}\|^2 + \|\mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\|^2 \right) = -c_i \quad \Rightarrow \quad\lambda = \frac{c_i}{\|\mathbf{E} \tilde{\mathbf{x}}_{1i}\|^2 + \|\mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\|^2}\]
最后，将 \(\delta_1, \delta_2\) 代回目标函数：

\[\|\delta_1\|^2 + \|\delta_2\|^2 = \lambda^2 \left( \|\mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\|^2 + \|\mathbf{E} \tilde{\mathbf{x}}_{1i}\|^2 \right) = \frac{c_i^2}{\|\mathbf{E} \tilde{\mathbf{x}}_{1i}\|^2 + \|\mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\|^2}\]
这正是 Sampson 误差：

\[\epsilon_i(\mathbf{E}) = \frac{(\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i})^2}{\|\mathbf{E} \tilde{\mathbf{x}}_{1i}\|^2 + \|\mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\|^2}\]
该式具有明确的几何意义：分子衡量对极约束的违反程度，分母则归一化了对极线的方向（避免因线方向不同导致的尺度偏差）。在小噪声假设下，Sampson 误差是真实重投影误差的最佳一阶近似。
于是，非线性最小二乘问题为：

\[\min_{\mathbf{E}} \quad S(\mathbf{E}) = \sum_{i=1}^N \epsilon_i(\mathbf{E}) = \sum_{i=1}^N \frac{(\tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i})^2}{\|\mathbf{E} \tilde{\mathbf{x}}_{1i}\|^2 + \|\mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\|^2}\tag{11}\]
与标准的非线性最小二乘问题不同，对极几何估计是一个隐式约束问题——它没有显式的模型输出形式 \(y = f(\boldsymbol{\theta}, x)\)。优化目标直接定义为 Sampson 误差 \(\epsilon_i(\mathbf{E})\)，而非某个预测残差的平方。在实际实现中（例如使用 Ceres Solver），应将残差设为 \(\sqrt{\epsilon_i}\)，这样优化器最小化的正是 \(\sum_i \epsilon_i\)。
相应地，残差的雅可比矩阵应为 \(\displaystyle \frac{\partial \sqrt{\epsilon_i}}{\partial \mathbf{e}}\)，其中 \(\mathbf{e} = \mathrm{vec}(\mathbf{E})\)。这一导数不仅依赖于约束违反量 \(c_i = \tilde{\mathbf{x}}_{2i}^\top \mathbf{E} \tilde{\mathbf{x}}_{1i}\) 对 \(\mathbf{e}\) 的变化，还包含分母项 \(\|\mathbf{E} \tilde{\mathbf{x}}_{1i}\|^2 + \|\mathbf{E}^\top \tilde{\mathbf{x}}_{2i}\|^2\) 的梯度，因此结构较为复杂。
然而，中间量 \(c_i\) 对 \(\mathbf{e}\) 的雅可比具有简洁的解析形式，且与线性 8 点法中的设计矩阵完全一致：

\[\frac{\partial c_i}{\partial \mathbf{e}^\top} = \begin{bmatrix}\tilde{u}_{1i} \tilde{u}_{2i} &\tilde{u}_{1i} \tilde{v}_{2i} &\tilde{u}_{1i} &\tilde{v}_{1i} \tilde{u}_{2i} &\tilde{v}_{1i} \tilde{v}_{2i} &\tilde{v}_{1i} &\tilde{u}_{2i} &\tilde{v}_{2i} &1\end{bmatrix}\]
出于计算简便或教学目的，一些方法会采用这一简化雅可比进行近似求解。但在高精度应用中，推荐依赖自动微分工具（如 Ceres）直接计算完整的 \(\partial \sqrt{\epsilon_i} / \partial \mathbf{e}\)，以确保收敛性和几何准确性。

Sampson 误差可以理解为对真实重投影误差的一阶泰勒近似。在噪声较小且初值合理的情况下，它能够以较低计算代价获得接近几何最优的估计。因此，它常被用于两视图几何的非线性优化阶段，为后续以真实重投影误差为目标函数的 Bundle Adjustment（BA） 提供稳定的初始化。

4. 实例

为验证前文所述对极几何估计方法的有效性，我们设计并实现了一个仿真案例：首先构建具有已知相对位姿的双目相机系统，然后在场景中随机生成 3D 点云，将其投影至两幅图像，并在像素坐标上叠加符合实际传感器特性的高斯噪声；最终，仅利用这些带噪的 2D-2D 匹配点，通过 8点算法初值估计 → 基于 Sampson 误差的非线性优化 → 本质矩阵流形投影 的三阶段流程，恢复出相对位姿所对应的本质矩阵 \(\mathbf{E}\)，并与地面真值进行多维度对比评估。具体实现代码如下：
[code]#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)