视觉里程计（VO）基础：对极几何、本质矩阵与基础矩阵、PnP问题、ICP算法、单目与双目VO实现

各位同学，欢迎来到视觉里程计这一章。说实话，VO是整个视觉SLAM系统的“发动机”。没有它，后面的后端优化、回环检测全是空中楼阁。我当年刚入门时，觉得VO不就是算个位姿嘛，结果被对极几何和本质矩阵折腾了好几个通宵。今天咱们就把这块硬骨头啃下来。

一、对极几何：两帧图像之间的几何约束

先问大家一个问题：为什么两张不同视角的照片能算出相机的运动？

答案就在对极几何里。说白了，对极几何描述的是两个相机坐标系下，同一个空间点投影到两幅图像上的几何关系。我习惯把它理解成“一个点、两条线、三个平面”的故事。

假设你在第一帧图像上看到一个特征点p1，它在第二帧图像上的对应点p2一定位于一条特定的直线上——这条线叫“极线”。这个约束关系，就是对极约束。数学上可以写成：

p2^T * F * p1 = 0

其中F就是基础矩阵。嗯，这里要注意：基础矩阵是3x3的矩阵，秩为2，自由度为7。为什么是7？因为尺度等价性去掉一个自由度，秩为2又去掉一个。我在项目中遇到过有人直接用8点法算F矩阵，结果因为点对质量差，算出来的矩阵秩不为2，整个位姿估计就崩了。

我的小技巧： 算基础矩阵前，一定要对特征点做归一化处理。这是Hartley在1997年提出的方法，能显著提高数值稳定性。我每次写代码都会先做这一步，省得后面调试到怀疑人生。

二、本质矩阵与基础矩阵：到底有什么区别？

很多初学者搞不清E矩阵和F矩阵的区别。我当年也迷糊过一阵子。其实很简单：

  • 本质矩阵E：工作在归一化图像坐标系下，包含了相机的旋转R和平移t。它只与相机的外参有关，和内参无关。
  • 基础矩阵F：工作在像素坐标系下，包含了相机的内参和外参。它是E矩阵左乘右乘内参矩阵的结果。

它们的关系是：F = K^(-T) * E * K^(-1)

我个人习惯：如果相机内参已知（比如用张正友标定法标定过），直接用E矩阵。因为E矩阵只有5个自由度（3旋转+3平移，去掉尺度），用5点法就能解，比8点法更鲁棒。我曾经在纹理稀疏的场景下，8点法直接炸了，换成5点法反而稳住了。

避坑指南： 解出E矩阵后，一定要做SVD分解并强制其奇异值为[1,1,0]。我见过有人忘了这步，结果分解出来的R和t完全不对，地图建出来全是扭曲的。

三、PnP问题：从3D到2D的位姿求解

当你知道一些3D点的空间坐标，以及它们在当前图像上的2D投影，怎么求相机的位姿？这就是PnP（Perspective-n-Point）问题。

PnP的解法有很多：P3P、EPnP、UPnP、直接线性变换（DLT），还有非线性优化的BA方法。我推荐大家优先掌握EPnP，它在精度和速度之间取得了很好的平衡。EPnP的核心思想是把3D点表示为4个控制点的加权和，然后求解控制点在相机坐标系下的坐标。

代码实现上，OpenCV提供了现成的函数：

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess=false, flags=cv::SOLVEPNP_EPNP);

嗯，这里有个坑：objectPoints和imagePoints的对应关系一定要正确。我见过有人把点的顺序搞反了，结果算出来的位姿完全相反，机器人直接往墙上撞。

核心要点： PnP至少需要3个点（P3P），但点越多越鲁棒。我建议至少用6个以上的点，并且用RANSAC剔除外点。在ORB-SLAM中，作者就是用EPnP+RANSAC的组合，效果非常稳定。

四、ICP算法：当你有两组3D点云时

ICP（Iterative Closest Point）用于对齐两组3D点云。在视觉SLAM中，常见于RGB-D相机或双目相机的场景。ICP的目标是找到一个旋转R和平移t，使两组点云的误差最小。

ICP的流程很简单：

  1. 对每个源点云中的点，找到目标点云中的最近邻点
  2. 根据匹配对计算R和t（可以用SVD分解）
  3. 应用变换，重复步骤1和2，直到收敛

但实际用起来坑不少。我记得有一次做室内重建，ICP怎么迭代都不收敛。后来发现是初始位姿太差，点云重叠区域太小。解决办法是用特征匹配先给一个粗略的初始位姿，再用ICP精化。

我的经验： ICP对初始值非常敏感。如果你用ICP做帧间匹配，建议先用视觉特征（比如ORB特征）估计一个初始位姿，再跑ICP。另外，别忘了加一个最大距离阈值，避免把离群点匹配上。

五、单目VO与双目VO的实现

终于到了实战环节。单目VO和双目VO，实现思路完全不同。

单目VO

单目VO的核心问题是尺度不确定性。你从对极几何算出来的平移t，只有方向没有大小。我刚开始做单目VO时，建出来的地图完全不知道有多大，感觉像在玩一个没有比例尺的玩具。

解决方法是：要么用固定大小的物体（比如已知尺寸的标定板）恢复尺度，要么用三角化后的3D点深度信息来约束。在实际系统中，单目VO通常需要初始化——先通过两帧图像恢复初始的3D点云和位姿，然后后续帧用PnP跟踪。

代码框架大致如下：

// 1. 提取特征并匹配
// 2. 用对极几何求初始位姿（E矩阵分解）
// 3. 三角化得到初始3D点
// 4. 后续帧用PnP求位姿
// 5. 局部BA优化

双目VO

双目VO就舒服多了。左右相机基线已知，深度可以直接通过视差算出来。每个特征点都有真实的3D坐标，不存在尺度问题。

我建议的实现流程：

  1. 左右图像分别提取特征
  2. 立体匹配，计算视差和深度
  3. 用3D-3D的ICP或3D-2D的PnP求帧间位姿
  4. 维护一个局部地图，做滑动窗口优化
注意： 双目VO的精度受基线长度影响很大。基线太短，深度误差大；基线太长，左右视图重叠区域小。我一般选基线在10-20cm之间，适合室内场景。室外场景可以适当加长。

六、总结与避坑清单

好了，这一章的内容就这些。最后给大家列一个我踩过的坑清单：

  • 特征匹配质量：别用暴力匹配，用FLANN或快速近似最近邻。匹配后一定要做交叉验证和比率测试。
  • 数值稳定性：解E矩阵、F矩阵前，一定要归一化坐标。这是血的教训。
  • 尺度问题：单目VO一定要处理好尺度漂移。我建议每隔一段时间用已知尺寸的物体做一次尺度校准。
  • 异常处理：当特征点太少或匹配质量太差时，宁可丢帧也别强行估计位姿。我曾经因为强行估计，导致整个地图崩了，不得不从头开始。

下一章我们会讲后端优化，到时候这些VO的结果会作为输入，交给图优化框架去精化。大家先把VO的基础打牢，后面就顺了。

← 上一章 📖 返回目录 下一章 →