4. 对极几何与本质矩阵：从两帧图像中恢复运动

做SLAM这么多年，我始终觉得对极几何是视觉里程计里最优雅的部分。它解决了一个核心问题：给定两帧图像，我们怎么知道相机动了多少？说白了，就是通过匹配的特征点，反推出相机的旋转和平移。

嗯，这一节内容不少，但别急。我们先从最直观的对极约束讲起，然后一步步深入到本质矩阵、基础矩阵，最后聊聊八点法和三角化。我保证，每块内容都会结合我实际踩过的坑来讲。

4.1 对极约束：两帧之间的几何关系

想象一下，你拿着相机拍了两个不同位置的画面。空间中的同一个点P，在左图上是p1，在右图上是p2。那么p1、p2和两个相机中心之间，存在一个天然的几何约束。

这个约束长什么样？

p2^T * t^ * R * p1 = 0

其中t是平移向量，R是旋转矩阵。t^表示t的反对称矩阵。这个公式看着简单，但它背后蕴含的信息量很大——它告诉我们，一对匹配点之间必须满足这个关系，否则就是误匹配。

我在项目中遇到过一个问题：有时候特征点匹配得挺好，但用对极约束一检查，发现大量外点。后来排查发现，是相机标定参数不准导致的。所以啊，对极约束的精度，很大程度上依赖于内参的准确性。

4.2 本质矩阵E与基础矩阵F

对极约束里有两个关键矩阵：本质矩阵E和基础矩阵F。它们有什么区别？

矩阵	定义	使用场景	自由度
本质矩阵E	E = t^ * R	已知相机内参（归一化坐标）	5
基础矩阵F	F = K^(-T) * E * K^(-1)	未知或使用像素坐标	7

说白了，E是F的「去内参」版本。如果你已经标定了相机，直接用E更简洁。如果没标定或者想用像素坐标，那就用F。

我个人习惯在SLAM系统里直接用E。为什么呢？因为大多数情况下我们都有标定好的相机，用E可以减少一个未知量，求解更稳定。

4.3 八点法求解：从匹配点到本质矩阵

好了，理论讲完了，怎么实际求解？最经典的方法就是八点法。

八点法的思路很简单：把对极约束写成线性方程组的形式。每个匹配点提供一个方程，8个点就能解出E的9个元素（去掉尺度因子）。

具体步骤是这样的：

1. 对匹配点坐标做归一化（这一步很重要，我吃过亏）
2. 构建系数矩阵A，大小为8×9
3. 对A做SVD分解，取最小奇异值对应的右奇异向量
4. 将得到的向量重排成3×3矩阵
5. 再做一次SVD，强制奇异值为[σ, σ, 0]的形式

代码实现大概长这样：

def estimate_E(points1, points2, K):
    # 归一化坐标
    pts1_norm = np.linalg.inv(K) @ points1.T
    pts2_norm = np.linalg.inv(K) @ points2.T
    
    # 构建A矩阵
    A = np.zeros((len(points1), 9))
    for i in range(len(points1)):
        x1, y1 = pts1_norm[0,i], pts1_norm[1,i]
        x2, y2 = pts2_norm[0,i], pts2_norm[1,i]
        A[i] = [x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1]
    
    # SVD求解
    U, S, Vt = np.linalg.svd(A)
    E = Vt[-1].reshape(3, 3)
    
    # 强制奇异值约束
    U, S, Vt = np.linalg.svd(E)
    S = np.diag([1, 1, 0])
    E = U @ S @ Vt
    
    return E

4.4 单应矩阵H：平面场景的利器

本质矩阵假设场景是三维的。但如果场景近似一个平面（比如无人机拍地面、墙壁），那用单应矩阵H更合适。

单应矩阵描述的是两个平面之间的映射关系：

p2 = H * p1

H有8个自由度（尺度不确定），所以至少需要4个点就能求解。求解方法和八点法类似，也是构建线性方程组做SVD。

什么时候用E，什么时候用H？我有个简单的判断方法：

• 如果场景深度变化大（比如室内有远近物体），用E
• 如果场景近似平面（比如桌面、墙面），用H
• 如果拿不准，两个都算，看哪个重投影误差小

我记得有一次做AR应用，相机对着桌面移动。用E解出来的运动一塌糊涂，换成H后瞬间准确了。原因就是桌面近似平面，H的模型更匹配。

4.5 三角化恢复深度：从2D到3D

有了R和t，我们就能恢复特征点的三维坐标了。这个过程叫三角化。

三角化的原理很简单：已知两个相机位姿和匹配点，求两条射线的交点。但实际中射线不会完美相交，所以要用最小二乘法。

常用的方法是线性三角化：

def triangulate(pose1, pose2, pt1, pt2):
    # 构建A矩阵
    A = np.zeros((4, 4))
    A[0] = pt1[0] * pose1[2] - pose1[0]
    A[1] = pt1[1] * pose1[2] - pose1[1]
    A[2] = pt2[0] * pose2[2] - pose2[0]
    A[3] = pt2[1] * pose2[2] - pose2[1]
    
    # SVD求解
    _, _, Vt = np.linalg.svd(A)
    X = Vt[-1]
    X = X / X[3]  # 齐次坐标转非齐次
    
    return X[:3]

我在实际项目中还发现一个问题：如果相机是纯旋转运动（没有平移），三角化会完全失效。因为此时两条射线平行，无法确定深度。所以SLAM系统里一定要保证有足够的平移分量。

4.6 本章知识体系

为了让你更直观地理解这些概念之间的关系，我画了一张流程图：

这张图把整个流程串起来了。从输入的两帧图像开始，先判断场景特性，然后选择E或H，最后分解出运动并恢复深度。每一步都有对应的数学工具和求解方法。

好了，对极几何这部分就讲到这里。内容不少，但核心就三件事：理解对极约束、掌握E和F的区别、会用八点法和三角化。这些是视觉SLAM的基石，后面讲图优化和回环检测时还会用到。

---

公众号:蓝海资料掘金营，微信deep3321