双目测距实战4-自标定

假设对于同一个景点，有很多人在不同位置进行了拍摄，如何根据这些照片来重建3D呢？

已知信息：相机的内参，焦距等

未知信息：相机的外参

未校准双目问题第一步就是校准相机，其实就是找个两个相机相对位置关系（旋转、平移）

可以靠拍摄照片中的匹配点找，这里最少需要八个点，可以利用这些点的对应关系，计算外参

简单来说，使用10到20组图像对和匹配点来标定双目相机，其原理是通过大量的、多角度的观测数据建立一个超定的方程组，从而以统计上最优的方式求解出相机的内外参数，以抵抗单次测量的噪声和误差。

下面我们来详细拆解这个原理：

1. 核心数学模型：对极约束 (Epipolar Constraint)

双目相机标定的数学基础是对极几何。对于空间中的一个点 P ，它在左、右相机图像上分别成像为点 p 和 p' 。这三个点（P 、p 、p' ）以及两个相机的光心 O 和 O' 必然位于同一个平面上，这个平面被称为"对极平面"。

这个共面关系可以用一个数学公式来表达，即对极约束：

p'T * F * p = 0

p 和 p' 是左右图像上对应匹配点的齐次坐标（已知量）。
F 是一个 3x3 的矩阵，被称为基础矩阵 (Fundamental Matrix)。它包含了两个相机之间的全部几何关系，即相机内参和外参（旋转R和平移T）。

我们的核心目标，就是要求解出这个基础矩阵 F ，然后从 F 中分解出我们需要的相机内外参数。

2. 为什么需要多个匹配点？------ 解方程

观察上面的对极约束方程，每一个匹配点对（p , p' ）都能提供一个关于 F 矩阵元素的线性方程。

基础矩阵 F 有9个元素，但由于尺度等价性（乘以任意非零常数后约束依然成立）和其内在性质（其行列式为0），它实际上只有7个自由度。
为了解出这些未知数，理论上我们至少需要7对匹配点。
在实际应用中，为了简化计算和提高稳定性，通常采用**"八点法" (Eight-Point Algorithm)**，即使用8对匹配点来构建一个线性方程组求解 F。

所以，从理论上讲，仅用一对双目图像和其上的8个匹配点，就可以计算出一个基础矩阵 F。

3. 为什么需要10到20组图像对？------ 提高精度和鲁棒性

既然8个点就够了，为什么还要用那么多组图像呢？关键原因在于现实世界中的噪声和误差。

对抗噪声，建立超定系统 ：
- 在真实图像中，我们通过算法找到的特征点位置不可能是百分之百精确的，总会存在几个像素的误差。这种误差就是"噪声"。
- 如果只用8个点，任何一个点的微小误差都可能导致计算出的 F 矩阵产生巨大的偏差。
- 通过使用成百上千的匹配点（来自10-20组图像），我们可以建立一个超定方程组 （方程的数量远大于未知数的数量）。这个系统没有精确解，但我们可以通过最小二乘法等优化算法，找到一个最符合所有这些数据点的"最优解"。这个解能最大程度地抵消随机噪声的影响，使得结果更加准确和稳定。
避免退化配置 (Degenerate Configurations) ：
- 在某些特殊情况下，即使有足够多的点，也无法得到唯一且正确的解。例如，如果所有的特征点都来自同一个平面，或者相机只进行了纯粹的旋转而没有平移，就会导致解的模糊性。
- 通过拍摄10到20组不同姿态和角度的图像，我们可以获得来自不同空间分布的特征点，提供更丰富的几何约束。这打破了单一视图可能带来的退化配置，确保了求解的唯一性和正确性，特别是对于精确求解相机的内参（如焦距和畸变系数）至关重要。
剔除错误匹配 (Outliers) ：
- 特征匹配算法并非完美，总会产生一些错误的匹配点对。这些"局外点"会对最小二乘法造成严重干扰。
- 有了大量的数据点，我们就可以使用像 RANSAC (随机抽样一致性) 这样的鲁棒算法。RANSAC会随机抽取一小组点（例如8个）来估计一个模型，然后用所有数据来验证这个模型，并重复这个过程。最终，那个获得最多数据点支持的模型会被采纳，从而有效地剔除了错误匹配点的影响。

如何解决错误的匹配点

在得到一系列初步的特征匹配点后，确实会有很多错误的匹配（称为"外点"或"离群点"，Outliers）。这些错误匹配会严重干扰后续的相机参数估计，导致结果不准确。因此，剔除错误匹配点是双目相机自标定（以及任何需要几何模型估计的视觉任务）中至关重要的一步。

其核心思想是：假设存在一个正确的几何模型（如对极几何），那么绝大多数正确的匹配点（"内点"，Inliers）会符合这个模型，而错误的匹配点则不会。 我们可以利用这个差异来区分它们。

最常用和最强大的算法是 RANSAC (Random Sample Consensus，随机抽样一致性)。

RANSAC 算法原理及步骤

RANSAC 是一种迭代的、非确定性算法，用于从包含大量离群点的数据中估计数学模型的参数。它基于以下假设：

"内点"可以（用一个模型）解释，并且其参数可以通过少量的内点进行估计。
"外点"不符合模型。
存在一个过程，可以评估模型与数据点的一致性。

以下是 RANSAC 算法应用于剔除双目匹配点外点的基本步骤：

确定模型和最小样本集大小：
- 模型： 对于双目匹配点，我们要估计的模型通常是基础矩阵 (Fundamental Matrix, F)。
- 最小样本集： 估计基础矩阵 F 至少需要 7对或8对 匹配点（7点法或8点法）。我们通常取8对。
迭代过程： 算法会重复执行以下步骤很多次。
- 随机抽取样本： 从所有匹配点中随机抽取一个最小样本集（例如，8对匹配点）。
- 估计模型参数： 使用这个最小样本集来计算一个临时的基础矩阵 F_temp。
- 评估所有点： 对于数据集中的所有匹配点，计算它们与 F_temp 的几何一致性（即，这些点是否符合 F_temp 所定义的对极约束）。通常使用距离度量 来衡量，例如：
  - Sampson Distance (Sampson距离)：衡量点到其对应对极线的距离。
  - Symmetric Epipolar Distance (对称对极距离)：左右图像点到各自对极线的距离之和。
  - 如果一个匹配点的误差（距离）小于预设的阈值 T，则认为它是一个内点，支持当前的 F_temp 模型。否则，它就是外点。
- 统计内点数量： 统计有多少匹配点被认为是内点。
选择最佳模型： 在所有迭代中，选择那个拥有最多内点 的模型 F_best 作为最优模型。
最终模型优化： 一旦找到最佳模型 F_best 和其对应的所有内点，通常会使用这些内点（现在被认为是干净的数据）来重新估计 一次模型 F_best。这次估计可以使用更精确的优化方法（例如，最小二乘法，因为它现在面对的是一个相对"干净"的数据集），以获得更精细的参数。

为什么 RANSAC 如此有效？

鲁棒性： RANSAC 最大的优势在于它对大量外点具有极高的鲁棒性。即使数据集中有超过50%甚至更多的外点，它仍然能够找到正确的模型。这是因为它每次只用极小部分的点来构建模型，降低了被外点"污染"的概率。
非确定性： 每次运行的结果可能略有不同，但只要迭代次数足够多，找到最优模型的概率就会非常高。

RANSAC 的关键参数和考虑

阈值 (Threshold T)： 决定一个点是内点还是外点的临界值。选择太小可能导致好的匹配点被剔除，选择太大可能导致坏的匹配点被误判为内点。通常根据图像噪声和期望的精度来确定（例如，1-3像素）。
迭代次数 (Number of Iterations)： 迭代次数越多，找到最佳模型的概率就越高，但计算成本也越高。迭代次数可以根据以下公式估算：
N = log(1 - p) / log(1 - w^m)
其中：
- N 是迭代次数。
- p 是希望找到正确模型的概率（例如，0.99）。
- w 是数据集中内点的比例（事先未知，但可以估计）。
- m 是估计模型所需的最小样本点数（例如，8）。
模型选择： 除了基础矩阵，也可以直接估计本质矩阵 E (如果已知内参)，或者单应性矩阵 H (如果所有点都在一个平面上)。

其他一些剔除外点的方法（作为补充）

LMedS (Least Median of Squares，最小中值平方误差法)： 类似于 RANSAC，但它最小化的是所有点误差的中值，而不是均方误差，因此对单个大的外点不敏感。计算成本通常高于 RANSAC。
MSAC (M-estimator SAmple Consensus)： RANSAC 的变种，在评分函数上有所改进，通常比标准 RANSAC 表现更好。
PROSAC (Progressive SAmple Consensus)： 针对特征匹配点通常有"质量"得分（如SIFT/SURF匹配的距离）的特点，PROSAC 优先从高质量的匹配点中采样，可以更快地找到好的模型。
图割 (Graph Cut) 或聚类方法： 在某些情况下，可以通过构建图模型或聚类来将匹配点分成符合某个几何变换的组。但对于基础矩阵这类全局模型，RANSAC 仍然是最主流和高效的方法。
描述子距离比率测试 (Descriptor Distance Ratio Test)： 这通常是RANSAC之前 的初步过滤。例如，对于SIFT或SURF匹配，如果最佳匹配的描述子距离与次佳匹配的描述子距离之比大于某个阈值（例如0.7-0.8），则认为这是一个可靠的匹配。这能显著减少RANSAC的输入数据量和外点比例。

附录：极线几何的推导

这个推导过程的精髓在于：将"点、线、面"的几何关系，巧妙地翻译成向量和矩阵的代数运算。

首先，纠正并明确一下符号：

我们通常使用齐次坐标来表示图像点，即 p = [u, v, 1]^T 和 p' = [u', v', 1]^T。
正确的公式是 p'^T * F * p = 0（p'的转置乘以 F 再乘以 p）。这是一个矩阵乘法，其结果是一个标量（一个数字），这个数字等于0。

现在，我们开始分步推导。

第1步：建立两个相机坐标系下的3D关系

我们有两个相机坐标系：左相机和右相机。空间中的点 P 在这两个坐标系下有不同的坐标。

设 P 在左相机坐标系下的坐标为 P_l = [X, Y, Z]^T。
设 P 在右相机坐标系下的坐标为 P_r。

这两个坐标可以通过一个刚体变换 （旋转+平移）联系起来：
P_r = R * P_l + T

R：是一个3x3的旋转矩阵，表示右相机相对于左相机的旋转姿态。
T：是一个3x1的平移向量，表示右相机光心相对于左相机光心的位置。
R 和 T 就是我们最终想求的相机外参。

第2步：将几何约束"翻译"成向量运算

我们已经知道，三个关键的向量位于同一个平面（对极平面）上：

P_l：从左相机光心 O 指向空间点 P 的向量。
T：从左相机光心 O 指向右相机光心 O' 的向量。
P_r：从右相机光心 O' 指向空间点 P 的向量。

几何学知识：如果三个向量共面，那么它们构成的平行六面体的体积为0。在向量代数中，这可以用**标量三重积（Scalar Triple Product）**来表示：一个向量与另外两个向量的叉积做点积，结果为0。

因此，我们的共面约束可以写成：
P_r · (T × P_l) = 0

或者等价地写成：
P_l · (T × P_r) = 0

（注意：这里我们用 P_r = R*P_l + T 代入，会导致推导复杂。有一个更巧妙的方法，见下一步）

让我们使用第一种形式。这个公式就是"共面"这个几何事实的第一次代数翻译。

第3步：引入矩阵运算，构建本质矩阵 E

上面的标量三重积虽然正确，但不太好用。我们需要把它变成纯粹的矩阵乘法形式。这里有一个关键的代数技巧：向量的叉积可以等效为一个反对称矩阵的乘法。

对于向量 T = [Tx, Ty, Tz]^T，其对应的反对称矩阵 [T]_× 为：

plain 复制代码

    [ 0, -Tz,  Ty]
[T]_× = [ Tz,  0, -Tx]
    [-Ty,  Tx,  0 ]

这样，叉积 T × P_l 就可以写成矩阵乘法 [T]_× * P_l。

现在，我们把这个代换回第2步的公式：
P_r · ([T]_× * P_l) = 0

向量的点积 a · b 可以写成 a^T * b。所以：
P_r^T * ([T]_× * P_l) = 0

现在，我们把第1步的关系 P_r = R * P_l + T 代入。等一下，这样做会变复杂。我们回到最初的三个共面向量 P_l, T, 和 P_r。

更正一下思路，让我们重新审视共面的三个向量。它们是 P_l（左光心到P），T（左光心到右光心），以及 P_r（右光心到P）。这三个向量构成了对极平面。

将 P_r = R * P_l + T 变形一下得到 P_r - T = R * P_l。这个关系并不直观。

让我们换一种更经典、更清晰的推导路径：

我们有 P_r = R * P_l + T。
两边同时与 T 做叉积：T × P_r = T × (R * P_l + T)
因为一个向量和自己的叉积为0 (T × T = 0)，所以上式简化为：T × P_r = T × (R * P_l)
现在，两边同时与 P_r 做点积：P_r · (T × P_r) = P_r · (T × (R * P_l))
左边是三个向量 P_r, T, P_r 的标量三重积。因为有两个向量相同，所以结果必然为0。这意味着这三个向量共面，体积为0。
因此，我们得到 P_r · (T × (R * P_l)) = 0。
将点积和叉积写成矩阵形式：P_r^T * [T]_× * (R * P_l) = 0
整理一下：P_r^T * ([T]_× * R) * P_l = 0

我们把中间的 [T]_× * R 这个部分定义为一个新的 3x3 矩阵，称为本质矩阵 (Essential Matrix, E) 。
E = [T]_× * R

于是，我们的约束方程就变成了非常简洁的形式：
P_r^T * E * P_l = 0

第4步：从3D坐标到2D像素坐标，构建基础矩阵 F

到目前为止，我们用的还是3D空间坐标 P_l 和 P_r。但我们手头只有2D图像上的像素点 p 和 p'。我们需要建立它们之间的关系。

根据小孔成像模型，3D点坐标和2D图像的归一化坐标 x（即Z=1平面上的坐标）的关系是：
P_l = λ * x_l 和 P_r = λ' * x_r

（其中 λ 是深度）

而归一化坐标 x 和像素坐标 p 之间通过相机内参矩阵 K 关联：
p = K * x => x = K⁻¹ * p

现在，我们将这些关系代入到 P_r^T * E * P_l = 0 中：
(λ' * x_r)^T * E * (λ * x_l) = 0
λ' * λ * x_r^T * E * x_l = 0

因为深度值 λ 不是0，我们可以消掉它们：
x_r^T * E * x_l = 0

最后，把像素坐标 p 换进来：
(K_r⁻¹ * p')^T * E * (K_l⁻¹ * p) = 0
(p'^T * K_r⁻T) * E * (K_l⁻¹ * p) = 0 (这里 K⁻T 表示K的逆的转置)
p'^T * (K_r⁻T * E * K_l⁻¹) * p = 0

我们把中间这一大块 (K_r⁻T * E * K_l⁻¹) 定义为基础矩阵 (Fundamental Matrix, F) 。
F = K_r⁻T * E * K_l⁻¹ = K_r⁻T * [T]_× * R * K_l⁻¹

最终，我们就得到了最终的对极约束方程：
p'^T * F * p = 0

总结

p'^T * F * p = 0 这个方程之所以成立，是因为：

几何上，两个相机光心和空间点P必然共面。
代数上，这个共面关系可以用向量的标量三重积等于0来表示。
通过矩阵变换 ，这个标量三重积可以优雅地写成 P_r^T * E * P_l = 0 的形式，其中本质矩阵 E 封装了相机的旋转 R 和平移 T。
再通过相机内参 K 将不可直接观测的3D坐标 P 替换为可直接观测的2D像素坐标 p，最终得到了 p'^T * F * p = 0，其中基础矩阵 F 封装了两个相机的全部几何信息（内外参数）。

所以，这个简洁的方程，是双目视觉底层几何原理的完美代数体现。