视觉里程计前端（2D-2D）：对极约束、本质矩阵、单应矩阵、三角化与PnP
好，咱们今天聊点硬核的。视觉里程计的前端，说白了就是怎么从两张图里算出相机的运动。2D-2D 的情况，是咱们入门 SLAM 的第一道坎。我个人觉得，这部分理解透了，后面 IMU 融合才会顺。
对极约束：两张图之间的几何关系
想象一下，你拿着相机拍了两个不同位置的画面。同一个空间点，在两个图像上的投影点之间，存在一个天然的几何约束。这就是对极约束。
它的核心公式很简单：x₂ᵀ F x₁ = 0。其中 F 是基础矩阵，x₁ 和 x₂ 是归一化平面上的点。
为什么会有这个约束？因为两个相机中心、空间点、两个像点，这五个点共面。这个平面叫极平面。极平面与两个成像平面的交线，就是极线。
我刚开始做的时候，总觉得这公式是纯数学推导，离工程很远。直到有一次，我在室外跑数据集，特征点匹配错了，结果算出来的运动轨迹直接飞了。后来一查，就是对极约束没用好，没做 RANSAC 剔除误匹配。
核心要点：对极约束不依赖场景结构，只依赖相机运动。它是 2D-2D 估计的基础。
本质矩阵 E 与基础矩阵 F
本质矩阵 E 和基础矩阵 F，其实是一回事，区别在于是否已知内参。
- 本质矩阵 E：已知内参 K，E = t^ R。它描述的是归一化坐标系下的对极约束。
- 基础矩阵 F：未知内参，F = K⁻ᵀ E K⁻¹。它描述的是像素坐标系下的对极约束。
实际工程中，我们通常先算 E，因为内参是标定好的。E 矩阵有 5 个自由度（3 旋转 + 3 平移，但尺度不确定），所以理论上 5 对点就能解。不过实践中，8 点法更稳定。
我的经验：用 8 点法时，一定要先对数据做归一化。否则数值不稳定，算出来的 E 矩阵可能是个病态矩阵。我曾经因为这个坑，debug 了整整两天。
单应矩阵 H：平面场景的利器
单应矩阵 H 描述的是两个平面之间的映射关系。当场景近似一个平面，或者相机纯旋转时，对极约束会退化，这时候单应矩阵就派上用场了。
H 矩阵有 8 个自由度，4 对匹配点就能解。它的形式是：x₂ = H x₁。
你想想看，什么时候用 E，什么时候用 H？我个人的习惯是：先算 H，再算 E，然后比较两者的重投影误差。如果 H 的误差明显更小，说明场景近似平面，用 H 更靠谱。
注意：单应矩阵假设场景是平面。如果场景深度变化很大，用 H 会引入较大误差。我在做无人机俯视场景时，地面是平面，用 H 效果就很好。但换成室内杂乱环境，还是老老实实用 E。
三角化恢复深度
有了相机运动（R, t），我们就能通过三角化恢复 3D 点的深度。说白了，就是已知两个视角的投影，反推空间点的位置。
三角化的方法有很多，最常用的是线性三角化（DLT）。它的思路很简单：构建一个关于 3D 点 X 的线性方程组，然后解最小二乘。
嗯，这里要注意：三角化对平移很敏感。如果平移太小，深度估计会非常不稳定。我记得有一次，我用纯旋转的数据做三角化，结果深度全是无穷大。后来才意识到，纯旋转下三角化是病态的。
// 线性三角化示例（伪代码）
// 输入：两帧的投影矩阵 P1, P2，匹配点 u1, u2
// 输出：3D 点 X
Matrix4x4 A;
A.row(0) = u1.x * P1.row(2) - P1.row(0);
A.row(1) = u1.y * P1.row(2) - P1.row(1);
A.row(2) = u2.x * P2.row(2) - P2.row(0);
A.row(3) = u2.y * P2.row(2) - P2.row(1);
// 对 A 做 SVD 分解，取最小奇异值对应的右奇异向量
Vector4d X = svd(A).V.col(3);
X = X / X(3); // 齐次坐标归一化避坑指南：三角化之前，一定要检查视差角。视差角太小（比如小于 1 度），深度估计不可靠。我一般会设置一个阈值，视差角小于 0.5 度的点直接丢弃。
PnP 问题：从 3D-2D 估计运动
当我们已经有了 3D 地图点，就可以用 PnP（Perspective-n-Point）来估计当前帧的位姿。这是 3D-2D 的经典问题。
PnP 的方法很多，我重点说两个：DLT 和 EPnP。
DLT（直接线性变换）
DLT 是最直接的方法。它把位姿的 12 个未知数（R 的 9 个 + t 的 3 个）当作独立变量，构建线性方程组求解。每对 3D-2D 点提供 2 个方程，所以至少需要 6 对点。
DLT 的缺点很明显：它没有考虑旋转矩阵的正交约束。所以解出来的 R 可能不是正交矩阵，需要再做一次 SVD 投影到 SO(3) 上。
EPnP（Efficient PnP）
EPnP 是我个人比较喜欢的方法。它的核心思想是用 4 个控制点来表示所有 3D 点，然后通过控制点的投影关系来求解位姿。复杂度是 O(n)，效率很高。
EPnP 的精度在大多数场景下都够用。我做过对比测试，在噪声不大的情况下，EPnP 和迭代法（比如 Levenberg-Marquardt）的精度差不多，但速度快了一个数量级。
我的建议：如果追求实时性，用 EPnP 做初值，再用非线性优化（比如 BA）做精化。这样既快又准。我在嵌入式设备上就是这么干的，效果很好。
总结一下
2D-2D 的流程，说白了就是：
- 提取特征点，做匹配
- 用对极约束或单应矩阵估计运动
- 三角化恢复 3D 点深度
- 后续帧用 PnP 估计位姿
每一步都有坑，但踩过坑之后，你对 SLAM 的理解会更深。我个人觉得，这部分是 SLAM 的基石，值得花时间反复推敲。
好了，今天就聊到这。下一章咱们会讲 3D-3D 的 ICP 问题，以及怎么把 IMU 数据融进来。到时候见。