4. 视觉几何基础：对极几何、三角化、PnP问题、本质矩阵与基础矩阵、单应矩阵
各位同学，咱们今天聊点硬核的。视觉几何，说白了就是研究怎么从一堆2D图片里，把3D世界的信息给“抠”出来。我刚开始接触VIO的时候，觉得这部分全是公式，头大得很。后来做项目踩了坑才明白，这些几何关系就是VIO的“地基”，地基不稳，上面盖的楼（状态估计、优化）全得塌。
这一节，咱们把五个核心概念掰开揉碎了讲：对极几何、三角化、PnP、本质矩阵/基础矩阵、单应矩阵。嗯，内容不少，但别怕，咱们一个一个来。
核心思想： 视觉几何解决的是“从图像到空间”的映射问题。你看到的每个像素点，背后都对应着三维空间里的一条射线。我们的任务，就是找到这些射线之间的约束关系。
4.1 对极几何：两帧之间的“红线”
先说说对极几何。你想想看，相机在位置1拍了一张图，在位置2又拍了一张图。这两张图里，同一个3D点投影到了不同的像素位置。对极几何干的事，就是描述这两个像素点之间的约束关系。
我个人习惯把对极几何理解成“找红线”。什么意思呢？假设你在左图看到一个特征点，那它在右图对应的点，一定落在一条特定的直线上——这条线就叫极线。有了这个约束，搜索匹配点就从“全图瞎找”变成了“沿着一条线找”，效率高多了。
我的经验： 在VIO初始化阶段，对极几何是判断两帧之间有没有足够视差的关键。我曾经遇到过一个场景，相机几乎纯旋转，对极几何直接失效——因为纯旋转时极线约束退化了。这时候就得靠单应矩阵来救场。
4.2 本质矩阵与基础矩阵：E和F的“双胞胎”
本质矩阵E和基础矩阵F，这俩长得像，但内涵不同。我刚开始学的时候总搞混，后来记住一句话就再也没错过：E是“去畸变”版的F。
- 本质矩阵 E：工作在归一化坐标系下（已经去掉了内参的影响）。它只包含旋转R和平移t的信息。自由度是5（3旋转+3平移，但尺度不可知）。
- 基础矩阵 F：工作在像素坐标系下。它包含了内参K的信息。自由度是7（因为尺度+两个内参自由度被吸收了）。
它们的关系很简单：F = K-T E K-1。说白了，E和F之间就差一个内参矩阵的“翻译”。
避坑指南： 我曾经在标定内参不准的情况下，直接用F矩阵做初始化，结果平移方向完全错了。后来我学乖了——如果内参标定靠谱，优先用E矩阵；如果内参不确定，或者相机有畸变没完全校正，用F矩阵更鲁棒。
4.3 单应矩阵：平面场景的“救星”
单应矩阵H，描述的是两个平面之间的映射关系。什么时候用？当场景近似一个平面时（比如无人机俯拍地面、墙壁上的纹理），或者相机纯旋转时。
你想想看，如果场景是平面，那对极几何就“过约束”了——本来应该沿着极线找匹配，现在直接一个矩阵变换就能把点对应上。H矩阵有8个自由度（尺度归一化后），用4对匹配点就能解出来。
关键区别： 对极几何（E/F）适用于一般场景，但要求有视差。单应矩阵H适用于平面场景或纯旋转。在VIO初始化时，我通常会同时计算E和H，然后用一个叫“H/E分解”的方法来判断场景结构——如果H的得分更高，说明场景偏平面，初始化策略要调整。
4.4 三角化：从2D到3D的“逆推”
有了两帧之间的位姿（R,t），又有了匹配点，怎么恢复出3D点的坐标？这就是三角化干的事。
原理其实很简单：从两个相机光心分别发出一条射线，理论上这两条射线应该交于一点。但实际中因为噪声，它们往往不相交。所以我们要找一个点，让它到两条射线的“距离之和”最小。
常用的方法有两种：
- 线性三角化：用奇异值分解（SVD）解一个齐次线性方程组。速度快，但精度一般。
- 优化三角化：把重投影误差作为代价函数，用高斯-牛顿法迭代优化。精度高，但慢一点。
我的习惯： 在VIO初始化阶段，先用线性三角化快速得到初始值，然后用一次高斯-牛顿迭代精化。这样既保证了速度，又提升了精度。另外，三角化对两帧之间的视差很敏感——视差太小，深度估计的方差会爆炸。我一般会设置一个阈值，视差小于2个像素的点直接扔掉。
4.5 PnP问题：已知3D点求位姿
PnP（Perspective-n-Point）是三角化的“逆问题”。三角化是已知位姿求3D点，PnP是已知3D点求位姿。
具体来说：给你一组3D点（地图点）和它们在当前图像上的2D投影，求当前相机在世界坐标系下的位姿（R,t）。
常用的解法有：
| 方法 | 最少点数 | 特点 |
|---|---|---|
| P3P | 3对点 | 解析解，速度快，但容易受噪声影响 |
| DLT | 6对点 | 线性解法，简单粗暴 |
| EPnP | 4对点 | O(n)复杂度，适合点多的场景 |
| 优化法（BA） | 任意 | 精度最高，但需要好的初始值 |
在VIO里，PnP主要用于两个地方：一是初始化成功后，跟踪新帧时确定当前位姿；二是重定位时，根据已知地图恢复位姿。
我曾经踩过的坑： 有一次用P3P做初始化，结果因为匹配点有外点，解出来的位姿完全不对。后来我改用RANSAC + EPnP的组合——先用RANSAC筛掉外点，再用EPnP求解。这个组合在工程上非常稳定，推荐大家试试。
4.6 小结：这些几何关系怎么串起来？
好了，咱们捋一捋。在VIO初始化里，这些几何关系是这么配合的：
- 拿到两帧图像，先做特征匹配。
- 用对极几何（E或F）或者单应矩阵H，估计两帧之间的相对运动。
- 用三角化恢复出匹配点对应的3D坐标。
- 有了3D点之后，后续新帧来了，用PnP确定当前位姿。
说白了，这就是一个“从2D到3D再到2D”的闭环。每一步都有坑，但每一步也都有对应的“填坑”技巧。做VIO这么多年，我最大的体会就是：不要迷信任何一个单一的几何约束，多模态融合才是王道。
最后说一句： 这些几何关系，光看公式是不够的。我建议你找两帧实际图像，自己动手算一遍E矩阵、做一次三角化。纸上得来终觉浅，绝知此事要躬行——这话放在SLAM里，再合适不过了。
公众号:蓝海资料掘金营，微信deep3321