多视图几何中的矩阵估计与位姿求解

Topic: Multi-View Geometry
Keywords: Homography, Essential Matrix, Fundamental Matrix, PnP
Reference: Multiple View Geometry in Computer Vision

引言

多视图几何研究从多张图像恢复相机运动和场景结构，核心是三个 3×3 矩阵：单应矩阵 H、本质矩阵 E、基础矩阵 F。
它们对应不同的坐标系层级（像素/归一化）和场景约束（平面/一般）。理解关键在于自由度分析和算法适用条件。

1. 单应矩阵 H

1.1 定义与约束

单应矩阵描述两个图像平面间的投影变换：
$$x_2 = H x_1$$

自由度：8 DOF（9 个元素，尺度不重要）

适用场景：

场景共面（如地面、墙壁、书页）
纯旋转相机（无平移）
远距离场景（平移可忽略）

1.2 DLT（Direct Linear Transform）算法

最少点数：4 对应点

核心思想：将非线性投影方程线性化，通过 SVD 求解。

单应变换消去分母后，每对点贡献 2 个线性约束：

$$\begin{bmatrix}
-u_i & -v_i & -1 & 0 & 0 & 0 & u_i’u_i & u_i’v_i & u_i’ \\
0 & 0 & 0 & -u_i & -v_i & -1 & v_i’u_i & v_i’v_i & v_i’
\end{bmatrix} \mathbf{h} = 0$$

其中 $\mathbf{h}$ 是 $H$ 按行展开的 9 维向量。
构造 $A\mathbf{h} = 0$ 后 SVD 求解，最小奇异值对应的向量即为解。

关键点：

归一化坐标（均值 0，标准差 $\sqrt{2}$）显著提升数值稳定性
RANSAC 处理外点
退化检测：比较 H 与 F 的重投影误差

2. 本质矩阵 E

2.1 定义与约束

本质矩阵描述两个已标定相机间的极几何约束（归一化坐标）：
$$\tilde{x}_2^T E \tilde{x}_1 = 0$$

其中 $\tilde{x} = K^{-1} x$ 是归一化坐标。

数学形式：
$$E = [t]_\times R$$

反对称矩阵：
$$[t]_\times = \begin{bmatrix} 0 & -t_z & t_y \\ t_z & 0 & -t_x \\ -t_y & t_x & 0 \end{bmatrix}$$

关键特性：

秩为 2：$\text{rank}(E) = 2$
特殊奇异值结构：$\text{diag}(\sigma, \sigma, 0)$
自由度：5 DOF（$R$: 3, $t$方向: 2）
不依赖内参：纯相机运动

2.2 五点法算法

最少点数：5 对应点（理论最少）

核心约束：极线约束 $\tilde{x}_2^T E \tilde{x}_1 = 0$ + 本质矩阵奇异值结构约束

$$2E E^T E - \text{trace}(E E^T) E = 0, \quad \det(E) = 0$$

求解：
5 点确定 $E$ 的 4 维零空间，代入立方约束得多项式方程组，最多 10 个实数解。

优势：
理论最少点数，直接估计 E，数值稳定性好。

2.3 分解恢复位姿

SVD 分解：$E = U \cdot \text{diag}(1, 1, 0) \cdot V^T$（需先校正奇异值）

四组解：
$R \in {U W V^T, U W^T V^T}$，$t \in {U(:,3), -U(:,3)}$

其中 $W = \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix}$

Cheirality Check：
三角化测试点，要求深度 > 0，仅 1 组解满足。

2.4 内参依赖

E 矩阵要求输入归一化坐标 $\tilde{x} = K^{-1}x$，因此必须预先标定相机。
标定方法包括棋盘格标定或 SLAM 在线标定。

3. 基础矩阵 F

3.1 定义与约束

基础矩阵描述两个未标定相机间的极几何约束（像素坐标）：
$$x_2^T F x_1 = 0$$

关键特性：

秩为 2：$\text{rank}(F) = 2$
自由度：7 DOF（9 个元素 - 尺度 - 秩约束）
依赖内参：包含 K 的影响
估计方法：八点法 / 七点法

物理意义：

定义极线约束：点 $x_1$ 对应图像 2 中的极线 $l_2 = Fx_1$
对应点 $x_2$ 必在此极线上

3.2 与 E 矩阵的关系

$$F = K_2^{-T} E K_1^{-1}$$

反过来：
$$E = K_2^T F K_1$$

自由度差异：
$$\text{DOF}(F) - \text{DOF}(E) = 7 - 5 = 2$$

这 2 个自由度对应内参影响（焦距、主点等）。

关键理解：

E 是相机空间的“真实运动”
F 是图像空间的“投影运动”
F 混入了 K，无法直接分解出 R,t
应用场景：
E 矩阵：已标定，直接求 R,t → 用于 SLAM、AR
F 矩阵：未标定，做几何约束 → 用于图像匹配、SfM（Structure from Motion）

3.3 八点法算法

最少点数：8 对应点（线性求解）

极线约束 $x_i’^T F x_i = 0$ 展开为线性方程：

$$\begin{bmatrix} u_iu_i’ & u_iv_i’ & u_i’ & v_iu_i’ & v_iv_i’ & v_i’ & u_i & v_i & 1 \end{bmatrix} \mathbf{f} = 0$$

求解：
构造 $A\mathbf{f} = 0$，SVD 求解后将最小奇异值向量重塑为 $F$，再强制秩 2 约束（将最小奇异值置 0）。

归一化八点法（Hartley 1997）：
坐标归一化到均值 0、标准差 $\sqrt{2}$，显著提升稳定性。

特点：线性求解，无需内参，但 8 点对 7 自由度有冗余。

3.4 七点法算法

最少点数：7 对应点（理论最少）

将 $F$ 表示为两个基矩阵的线性组合 $F = \alpha F_1 + (1-\alpha) F_2$，代入秩约束 $\det(F) = 0$ 得三次方程，最多 3 个实根。
通过重投影误差选择最优解。

适用：RANSAC 最小集，但需处理多解。

3.5 F 无法直接分解位姿

原因：
$\text{DOF}(F) = 7$，而 $\text{DOF}(R, t) = 5$，多出的 2 个自由度来自内参影响。
F 只知道极线方向（像素空间），不知道焦距、主点等物理参数，无法还原真实相机运动。

解决：

已知 K 时：通过 $E = K_2^T F K_1$ 转换后分解
未知 K 时：只能做极线约束，无法恢复位姿

F 矩阵的实际用途：
虽然不能直接求 R,t，但 F 在未标定场景下有重要应用：

立体匹配：极线 $l_2 = Fx_1$ 将 2D 搜索降为 1D，提高匹配效率
极线校正：矫正图像对使极线变为水平线，简化匹配（stereoRectifyUncalibrated）
自标定：从多个 F 估计内参 K（Kruppa 方程），再转为 E 求位姿
投影重建：不需度量信息，直接三角化得投影坐标系下的 3D 结构

4. PnP 问题

4.1 问题定义

PnP (Perspective-n-Point) 求解 3D-2D 对应下的相机位姿：
$$x = K(RX + t)$$

输入：

3D 点 $X_i$（世界坐标系）
对应 2D 点 $x_i$（图像坐标）
内参 $K$（已知）

输出：

相机位姿 $R,t$

与八点法的本质区别：

维度	PnP	八点法
输入	3D-2D	2D-2D
输出	相机位姿	F矩阵
前提	已知场景结构	估计相机关系
应用	定位	重建

概念澄清：PnP vs 具体算法

PnP 是问题类别（从 n 个 3D-2D 对应求相机位姿），而 P3P、EPnP、DLT PnP 是求解算法：

P3P：最少点数（3 点），余弦定理，最多 4 解
EPnP：多点高效算法（≥4 点），控制点方法，$O(n)$ 复杂度
DLT PnP：直接线性化，需后续优化

类比：排序是问题，快速排序是算法；PnP 是问题，EPnP 是算法。

4.2 P3P 算法

最少点数：3 对应点

根据余弦定理建立三角形方程组（$x, y, z$ 为光心到三 3D 点的距离）：

$$\begin{cases}
x^2 + y^2 - 2xy\cos\gamma = c^2 \\
x^2 + z^2 - 2xz\cos\beta = b^2 \\
y^2 + z^2 - 2yz\cos\alpha = a^2
\end{cases}$$

其中 $a, b, c$ 为 3D 点间距离（已知），$\alpha, \beta, \gamma$ 为观测射线夹角（由 2D 观测计算）。
求解得到 $x, y, z$ 后计算 $R, t$。

特点：
最少点数，闭式解，但最多 4 个解需验证，数值不够稳定。

4.3 EPnP 算法

适用：n ≥ 4 个点

核心思想：将所有 3D 点表示为 4 个控制点的线性组合。

$$P_i^w = \sum_{j=1}^{4} \alpha_{ij} C_j^w \quad \Rightarrow \quad P_i^c = \sum_{j=1}^{4} \alpha_{ij} C_j^c$$

投影方程 $\lambda_i x_i = K \sum_{j=1}^{4} \alpha_{ij} C_j^c$ 结合控制点距离约束求解 $C_j^c$，再计算 $R, t$。

优势：
$O(n)$ 复杂度，非迭代，精度高。

4.4 DLT PnP 算法

直接线性化投影方程 SVD 求解，但未利用 $R$ 正交性。
实际常用 DLT 初值 + LM 优化最小化重投影误差。

PnP 应用：
AR 标志物定位、机器人导航、SLAM 重定位。

5. 算法横向对比

5.1 点法系列总结

算法	求解对象	最少点数	自由度	坐标系	需要内参
P3P	$R,t$	3	6	3D-2D	是
DLT-H	$H$	4	8	2D-2D	否
五点法	$E$	5	5	归一化	是
七点法	$F$	7	7	像素	否
八点法	$F$	8	7	像素	否

为什么点数不等于自由度？

八点法：冗余 1 个点，线性求解
七点法：恰好 7 个点，非线性求解

5.2 坐标系层级

像素坐标 x --[K^-1]--> 归一化坐标 x̃
    |                      |
    | 八点法                | 五点法
    ↓                      ↓
    F (7 DOF)             E (5 DOF)
    |                      |
    | E = K2^T F K1        | SVD分解
    ↓                      ↓
    E (需要K)             R,t (4组解)
                           |
                           | Cheirality
                           ↓
                          唯一R,t

5.3 场景适用性

场景	优先选择	原因
平面场景	$H$	自由度低，稳定
已标定一般场景	$E$	直接恢复 R,t
未标定一般场景	$F$	可做匹配和极线校正
纯旋转	$H$	F 退化
已知 3D 结构	PnP	直接定位

5.4 工程决策树

是否已知 3D 结构？
├─ 是 → PnP → R,t
└─ 否 → 是否平面场景？
    ├─ 是 → DLT → H
    └─ 否 → 是否已标定？
        ├─ 是 → 五点法 → E → R,t
        └─ 否 → 八点法 → F → (极线约束)

6. 关键问题

6.1 为什么增加点数无法解决模型问题

误区：用 100 个点从像素坐标估计 E

真相：
像素坐标满足 $x_2^T F x_1 = 0$，归一化坐标满足 $\tilde{x}_2^T E \tilde{x}_1 = 0$。
强行用像素坐标解 E 本质上是在拟合 F。

点数解决统计问题，内参解决层级问题。

6.2 为什么需要归一化坐标

数值稳定性（最重要）：
像素坐标范围大（如 0-1920），SVD 求解时矩阵条件数差。
归一化到均值 0、标准差 $\sqrt{2}$ 后，数值误差显著降低（归一化八点法的核心改进）。

消除内参影响：
归一化坐标 $\tilde{x} = K^{-1}x$ 表示单位焦距下的标准射线方向，与内参无关。
E 矩阵描述纯相机运动（5 DOF），必须用归一化坐标；F 矩阵混入内参（7 DOF），用像素坐标。

工程实践：

已标定：用归一化坐标 + 五点法求 E（更稳定，自由度低）
未标定：八点法求 F 时也要先归一化再反归一化（Hartley 标准流程）

6.3 退化情况处理

常见退化：
纯旋转（$t=0$，E/F 退化）、平面场景（H 优于 F）、共线点。

检测：
比较 F 和 H 的重投影误差，检查运动 baseline，GRIC 准则。
自动切换 H/F/E 或重新采集。

6.4 多解消歧

E 分解 4 组解：用 Cheirality check（三角化深度 > 0）
P3P 可有 4 组解：用重投影误差验证
七点法 1-3 个解：在 RANSAC 框架下自动选择最优

7. 工程实践

OpenCV 实现：

// 八点法求 F
cv::Mat F = cv::findFundamentalMat(pts1, pts2, cv::FM_RANSAC);

// 五点法求 E（已标定）
cv::Mat E = cv::findEssentialMat(pts1, pts2, K, cv::RANSAC);
cv::recoverPose(E, pts1, pts2, K, R, t);

// DLT 求 H
cv::Mat H = cv::findHomography(pts1, pts2, cv::RANSAC);

// EPnP
cv::solvePnP(pts3d, pts2d, K, distCoeffs, rvec, tvec);

关键细节：

坐标归一化提升稳定性
RANSAC 处理外点
五点法依赖准确 $K$
多视图 BA 优化全局一致性

8. 总结

核心要点：

H、E、F 对应不同约束：H（平面/旋转，8 DOF）、E（已标定，5 DOF）、F（未标定，7 DOF）
坐标系层级决定算法：像素坐标 → F，归一化坐标 → E
F 无法直接分解 R,t（混入内参 2 DOF，需转换为 E）
点数解决统计问题，内参解决几何层级
PnP 是 3D-2D 问题，直接求位姿

实践建议：

SLAM 优先五点法
图像拼接优先 DLT
未标定双目优先八点法
重定位/AR 优先 PnP
所有算法配合 RANSAC

参考文献：

Multiple View Geometry in Computer Vision, 2003, Richard Hartley & Andrew Zisserman
An Efficient Solution to the Five-Point Relative Pose Problem, CVPR 2004, David Nister