做视觉定位这些年，我发现自己被问得最多的一个问题不是“怎么标定相机”，也不是“换哪种特征点”，而是“VO到底是什么，它和SLAM到底哪里不一样”。很多刚入门的同学抱着“我要做一个SLAM”的心态来找我，结果折腾了半天，真正落地的其实只是VO加了一个回环检测的外壳。这篇关于视觉里程计三十年发展史的综述，把VO的定义、数学基础、技术演变和未来方向都梳理得很清楚。我读的时候做了不少批注，后来在室内机器人、无人机悬停这些实际项目里反复验证过其中的结论，现在把这份笔记整理出来。不管你是刚接触视觉定位的新手，还是已经在SLAM领域写了不少代码的工程师，这篇笔记应该都能帮你把VO这块的根基看得更透。
1. 到底什么是视觉里程计，它与SLAM差在哪
1.1 里程计这个词是哪里来的
“里程计”这个词，最早是从轮式机器人那边借来的。轮式机器人想知道自己走了多远，最简单的办法就是在轮子上装编码器，数轮子转了多少圈，再结合轮径算出行进距离。但这套方案在地面打滑、轮胎变形的时候就完全不可靠了。后来有人想，既然人走路的时候可以靠眼睛判断自己移动了多少，为什么机器人不能用相机来做同样的事？
于是就有了视觉里程计：通过分析连续图像帧之间的变化，估计相机（也就是机器人本体）的运动轨迹。它只关心短时间内的增量运动，换句话说，它解决的是“我从上一刻到现在，到底往哪个方向动了多少”这个问题。论文里对这种增量式的运动估计做了明确界定——VO是连续帧之间的位姿估计，而不是一次性对所有图像做联合优化。
1.2 VO、SFM和SLAM的边界
很多人会把VO和运动恢复结构（SFM）、SLAM混在一起说，但它们的关注点其实完全不同：
- SFM的目标是从一堆无序图像里重建出三维结构和相机位姿，属于离线处理，可以反复迭代优化，追求全局最优。
- VO的目标是实时地、增量地估计相邻时间点之间的运动，不要求全局一致性，只要求局部准确。
- SLAM则是在VO的基础上往前再走一步：不仅要估计轨迹，还要维护一个全局一致的地图，并且通过回环检测来修正累积漂移。
所以从实现角度看，VO完全可以当作SLAM的前端模块来用。很多SLAM系统里跑的那套帧间匹配和位姿估计，本质上就是VO。那么问题来了：既然SLAM更强，论文为什么还要花几十页篇幅专门讲VO？因为VO是SLAM里最基础、最容易出问题的部分——如果前端给的位姿就是偏的，后面再怎么回环优化都救不回来。
1.3 一个容易被忽略的边界条件
论文里反复强调一个点：VO的关键假设是相邻帧之间有足够的视差和重叠区域。这个假设看起来简单，但在实际工程中，它决定了一个VO系统能不能跑起来。比如相机做纯旋转运动时，没有平移，三角化条件失效，单目VO直接“失灵”；又比如机器人原地快速打转，帧间重叠太小，特征匹配全部失败，系统跟丢。这些边界条件不是一个“性能问题”，而是“能不能用”的问题。
提示：如果你只是在做实验Demo，跑通VO可能只要一下午。但如果在做实际系统，一定要把“纯旋转”“低纹理”“快速运动”这些极端场景列进需求里，否则现场表现会很难看。
2. 标准VO流水线：从图像到轨迹的四步拆解
2.1 特征提取：选角点还是选描述子
经典的VO系统几乎都是特征法的天下。第一步是从图像里挑出稳定的特征点。论文里提到的特征包括早期的角点检测器、尺度不变特征（SIFT类）、以及后来更适合实时系统的二进制描述子（ORB类）。选择的标准其实就两条：重复检测率要高，计算速度要快。
我个人的经验是，在做VO实验时最喜欢用角点加金字塔光流来跟踪。角点响应值选得合适，一帧图像抽出几百个点，连续帧之间的跟踪效果非常稳。如果场景光照变化剧烈，或者需要做大规模匹配重用，才考虑用二进制描述子。
2.2 帧间匹配：描述子匹配与光流跟踪之争
帧间匹配是VO性能的分水岭。两种主流做法：
- 描述子匹配：提取当前帧特征点的描述子，和上一帧的特征描述子做最近邻匹配。优点是重定位能力强，不怕大位移；缺点是计算量大、耗时，帧率一高就会成为瓶颈。
- 稀疏光流跟踪：假设像素亮度在连续帧间不变，用金字塔光流算法直接跟踪上一帧的特征点。优点是速度快，一帧几十个点用几百微秒就能跑完；缺点是对亮度变化和快速运动敏感。
论文从历史角度回顾了这两种路线，但我可以给一个非常工程化的建议：如果你的相机运动是平滑的、帧率稳定在30帧以上，优先选KLT光流；如果跳变较大（比如关键帧式采集，帧率不稳定），优先选描述子匹配。实测下来，在大多数室内机器人平台上，KLT的表现比描述子匹配更稳，而且不容易出现“匹配跳线”的情况。
2.3 运动估计：2D-2D、3D-2D、3D-3D分别怎么选
拿到匹配点对之后，接下来要估计相机的旋转和平移。论文把运动估计分成了三大类，每类的适用场景完全不同：
- 2D-2D：两张图像上匹配到的都是二维像素坐标。此时用对极几何求本质矩阵或者单应矩阵，再分解出旋转和平移。适合单目相机的起始帧，也就是还没有任何三维信息的时候。
- 3D-2D：已知空间点三维坐标，同时知道它在图像上的二维投影。用PnP算法求解相机位姿。单目VO在初始化后，后续每一帧都可以用上一帧三角化出来的三维点来解PnP，所以这是单目系统在正常跟踪阶段的惯用做法。
- 3D-3D：两个传感器（比如双目相机左右目，或者连续两帧由深度相机直接给出深度）分别重建出三维点云，用迭代最近点（ICP）求两个点云之间的刚体变换。立体相机系统常用这种方案。
2.4 局部优化：为什么VO也要做Bundle Adjustment
很多人以为VO只是帧间递推，算完一帧就把结果丢掉。实际上，成熟VO系统都会保留一个滑动的局部窗口，里面装着最近N帧的位姿和它们看到的路标点，然后做局部的光束平差法（Bundle Adjustment，BA）优化。优化目标是最小化所有路标点的重投影误差总和。这样做的好处是，局部噪声会被约束在窗口范围内平滑掉，而不是全部污染到下一帧。
论文里对BA的评价很高，认为它是让VO从“能跑”变成“跑得准”的关键一步。我在工程里的理解是：没有BA的VO，短期内看不出问题，跑了几十米后轨迹就开始飘了；加了局部BA之后，误差累积会明显变慢，尤其在转弯和加减速时，位姿的跳变会平滑很多。
注意：局部窗口的尺寸不是越大越好。窗口太大，计算量上去，而且远处的路标点质量下降；窗口太小，平滑效果不足。我常用的窗口大小是10到15个关键帧，在精度和实时性之间比较平衡。
3. 几何基础与实现细节：对极约束、PnP、三角化和局部BA
3.1 对极几何：为什么两帧就能求出运动
假设同一个空间点P在相邻两帧图像中分别成像为p1和p2。当这两个相机位姿存在旋转和平移关系时，p1和p2之间存在一个被称为极线约束的几何关系，用基础矩阵F可以写成：
p2^T F p1 = 0
如果相机已经标定，可以进一步把像素坐标转换到归一化平面上，这时约束化简为本质矩阵E的关系：
x2^T E x1 = 0
本质矩阵E包含旋转R和平移t的信息。解出E之后，再做一次SVD分解，就能恢复出R和t。这个过程在论文里属于VO的基础数学部分，但实际编码时会遇到几个坑。首先是E的尺度不确定性：E乘上任意非零常数依然满足约束，所以从E分解出来的t只有方向，没有尺度——这正是单目VO没有绝对尺度的数学根源。其次是分解出的R和t有四个组合解，必须用“点在两个相机前方”的约束筛选出唯一正确的一组。
我用PyTorch风格写下过一个简洁的八点法实现，核心逻辑如下：
import numpy as np def eight_point_essential(x1, x2): """ x1, x2: 归一化平面坐标, shape (N, 2) 返回本质矩阵 E """ N = x1.shape[0] ones = np.ones((N, 1)) X1 = np.hstack([x1, ones]) X2 = np.hstack([x2, ones]) # 构造线性方程组 A * e = 0 A = np.zeros((N, 9)) for i in range(N): u1, v1 = X1[i] u2, v2 = X2[i] A[i] = [ u2 * u1, u2 * v1, u2, v2 * u1, v2 * v1, v2, u1, v1, 1 ] # 最小二乘解对应最小奇异值的右奇异向量 _, _, Vt = np.linalg.svd(A) E = Vt[-1].reshape(3, 3) # 强制本质矩阵的奇异值约束: [1, 1, 0] U, _, Vt = np.linalg.svd(E) S = np.diag([1.0, 1.0, 0.0]) E = U @ S @ Vt return E3.2 三角化：从两帧匹配点恢复三维点
有了两帧的位姿之后，匹配到的点可以反向投影出去，通过三角化得到三维坐标。论文里提到线性三角化方法，实际上就是构造一个包含两帧投影矩阵的齐次线性方程组，用SVD求解。数学上很简洁，但工程上有一个致命问题：当两帧之间的平移很小时，也就是“短基线”条件下，三角化的深度估计会非常不稳定，误差随噪声急剧放大。
这就是为什么我在实际项目中会要求VO系统尽量保持足够的运动速度，避免长时间停滞或者极小位移的抖动。另一个工程小技巧是，不要用三角化出来的所有点去做下一帧的PnP，要过滤掉反投影误差过大的点，同时剔除深度为负或深度接近无穷远的点。这个过滤步骤往往比换一种求解算法更能提升精度。
3.3 PnP：单目系统真正的主角
单目VO完成了初始化（通常是前两帧通过2D-2D运动估计加三角化建立了初始三维点）之后，后续每一帧都有了一个可靠的先验：上一帧的关键帧有位姿，三维点也有粗略坐标。于是运动估计就自然地转化为PnP问题——已知一系列三维点及其在当前图像上的投影，求相机的旋转和平移。
PnP的线性解法很多，论文里主要讲述的是直接用DLT求解以及利用RANSAC剔除误匹配的思路。在我的实践中，更推荐先跑RANSAC + PnP，筛掉外点之后，再对全部内点做一次非线性优化，这样即使在特征匹配有少量错误的情况下，系统的鲁棒性也能保持得很好。
PnP里面有一个现成的工程实现细节：把像素坐标和三维点坐标都做归一化处理，可以显著改善数值稳定性。具体做法是把三维点平移到以质心为中心、再缩放到平均距离为1的尺度，像素坐标也做类似的操作。很多调试时“为什么SVD解出来是NaN”的问题，根源就在数值条件太差。
3.4 局部BA：用非线性优化把轨迹“磨平”
论文里的“fundamentals”部分对BA着墨甚多。BA的核心是构造一个重投影误差函数：
E(位姿, 路标) = Σ || z_ij - project(T_i, X_j) || ^ 2
其中z_ij是路标点X_j在第i帧图像上的实际观测像素坐标，project是相机投影模型。优化变量包括所有关键帧的位姿和所有路标点的三维坐标，目标是把误差总和降到最低。
求解BA的标准算法是Levenberg-Marquardt法，它结合了高斯牛顿法和梯度下降法的优点。实际框架里很难手写全套BA，工程上都是直接调现成优化库。论文的贡献在于明确告诉读者：BA不仅是为了SLAM后端服务的，在VO阶段就开始用BA，能有效把误匹配和噪声导致的跳变拉住，不至于让误差滚雪球。
4. 这30年发生了什么：从滤波器时代到优化时代的演变
4.1 早期：从载具导航需求出发的研究
论文对“第一个30年”的回顾，起点可以追溯到20世纪80年代初的视觉运动估计研究。那时候的研究动机主要来自车辆导航：能不能不依赖GPS，仅凭车上的相机就估算出车的位置和姿态？当时计算机视觉领域的核心工具是特征匹配加线性代数，整个系统跑起来很慢，但思路已经成形。
90年代中后期，行星探测漫游车的研究推动着VO走向实战。当时的小型行星车在野外环境测试视觉测程技术，用立体视觉加特征跟踪来实现六自由度运动估计。为什么行星车这么需要VO？因为火星上没有GPS，轮子容易在松软地面打滑，传统的轮式里程计基本不可靠。这一阶段的研究把VO从“能用”推向了“在恶劣环境下也必须能用”，对鲁棒性的要求明显提高。
4.2 从滤波器到关键帧优化：范式转换
大约2000年前后，实时视觉定位出现了一个重要的转折，状态估计从基于扩展卡尔曼滤波（EKF）的滤波框架，逐渐转向了基于关键帧和BA的优化框架。EKF的做法是把相机位姿和路标点都放进一个巨大的状态向量里，每次来一帧图像就更新一遍。理论上很漂亮，但计算量随路标点数量急剧增长，而且系统的线性化误差会累积，导致长期运行的精度受限。
后来的关键帧方案完全不同：不追求每一帧都更新全局状态，而是抽取出若干质量好的关键帧，只在这些关键帧上做局部BA。这样既保持了精度，又把计算量控制在了可以实时运行的范围。很多后来的单目SLAM系统采用的就是这一套思路。论文把这种转变评价为VO走向实用的关键一步，我也是这么理解：滤波方法在数学上更“完整”，但优化方法在工程上更“实用”。
4.3 特征法与直接法的路线分野
最后一个重要的历史脉络是特征法与直接法的分化。特征法先提取角点、描述子，再进行匹配；直接法跳过特征提取，直接用图像的像素灰度值构造误差函数。早期VO大多沿特征法的路线走，因为特征点匹配天然地能容忍一定的光照变化和几何变形，而且误匹配可以通过RANSAC剔除。
直接法在后来因为其低纹理场景下的优势重新受到重视，但它对灰度不变假设要求极高，一旦光照变化剧烈，误差函数很容易出现大量局部极小。论文对这两种路线的对比非常客观：各有优势，也各有软肋。在我做过的项目里，如果场景纹理丰富、光照稳定，直接法也能跑得很好；但只要场景一变，特征法的稳定性优势立刻就体现出来了。
5. 从论文走向工程：常见坑位与排查建议
5.1 工程中一定会踩的七个坑
理论讲得再多，最后还是要落到真机上。下面这张表是我把论文结论和自身项目经历结合后，总结出的高频问题速查表，按出现的频率从高到低排列：
| 现象 | 根因 | 建议方案 |
|---|---|---|
| 轨迹越来越短或越来越长 | 单目尺度漂移累积 | 增加局部BA；引入深度传感器或IMU约束尺度 |
| 快速旋转时跟丢 | 帧间重叠不足，特征匹配失败 | 提高帧率；降低运动角速度；增加短暂“回正”动作 |
| 纯旋转时位置不动，轨迹发飘 | 单目纯旋转无视差，三角化失效 | 检测纯旋转状态，停止更新位置；用IMU补偿旋转 |
| 阳光下或灯光变化时特征点大量丢失 | 光照变化使亮度假设或特征检测失效 | 改用描述子匹配；做图像增强；提高曝光自适应速度 |
| 低纹理走廊，特征点数量不足 | 墙面天花板缺乏角点 | 融合结构光/深度相机；减小特征点响应阈值并启用边缘点 |
| 运动模糊导致特征检测抖动 | 曝光时间过长或运动过猛 | 提高快门速度；增加光源；降低底盘速度 |
| 动态行人经过时轨迹被拉偏 | 移动物体特征点干扰本质矩阵求解 | 用语义分割剔除动态区域；或用鲁棒核函数降低外点影响 |
5.2 我的参数调试心得
很多刚上手的同学总想找到一套“万能参数”，这个思路在实践中行不通。以帧率为例，VO的基础假设是相邻帧视差不能太大也不能太小。视差太大，匹配失败多；视差太小，三角化不稳定。我之前在一个室内机器人项目里试过低帧率采集，比如10帧每秒，结果机器人稍微转快点，系统就彻底丢失。后来把帧率提到30帧以上，同样的算法代码几乎没怎么改，稳定性立刻改善。
另一个容易被忽略的参数是特征点的数量与分布。之前有同学问我“是不是特征点越多越准”，答案是否定的。特征点太多，一方面拖慢速度，另一方面大量分布集中的点并不会带来多少信息增益，反而可能让优化陷入局部极小。我常用的策略是每帧抽出200到500个点，同时强制在图像上分区域均匀抽取，避免所有点都堆在纹理丰富的物体上。实践下来，均匀分布对旋转运动时的估计精度提升非常明显。
5.3 关于单目尺度问题的持久战
单目VO有一个绕不过去的问题：无法获知真实的绝对尺度。平移向量t从本质矩阵分解出来时，天然地丢失了长度信息。后续帧的运动估计继承了这一不确定性，最终整个轨迹会被“拉伸”或“压缩”。局部BA可以抑制短期的尺度抖动，但无法根治长距离的尺度漂移。
在实际工程里，要根治尺度问题，通常有三种方案：一是加IMU，用加速度计信息约束尺度；二是加深度摄像头或激光雷达，直接给深度；三是利用已知大小的物体，比如环境中固定高度的路标，提供尺度先验。论文里虽然没有详细展开这些融合方案，但它把“尺度不可观测”这个源头讲清楚了，理解这一点之后，你会明白：单目VO的尺度问题不是代码bug，而是几何意义上的固有性质，只能靠信息融合来补。
5.4 动态环境下的实用对策
动态物体一直是很现实的麻烦。某次测试时，我特意让一个人从镜头前走过，结果系统输出的轨迹直接被拉偏了一个角度，这个偏差后来通过回环才勉强校正回来。原因很简单：本质矩阵求解时用了最小二乘，动态点作为外点没有被妥善剔除。
推荐的排查路径分三步：先检查RANSAC的阈值设置，能否筛掉大部分动态点；再检查特征点提取的区域分布，如果动态行人占据了图像大部分区域，单纯靠RANSAC也挡不住；最后是上语义分割模型，把行人、车辆这类动态目标在特征提取阶段就排除掉。论文不会教你这些工程经验，但这恰恰是让VO系统真正落地的关键。
6. 给后来者的阅读建议与我的读后体会
6.1 这篇论文应该怎么读
这篇综述信息密度很高，如果从头到尾逐字读，很容易在数学推导部分耗尽耐心。我建议按下面这个顺序来读：
- 先读引言和第二章的问题定义，搞明白VO到底解决什么问题，和SFM、SLAM的边界在哪里。
- 接着读特征提取与匹配的部分，这部分是工程入门基础。
- 然后读运动估计的几何基础，重点看本质矩阵、PnP、三角化和BA的关系。
- 最后浏览历史回顾章节，这部分更像是技术编年史，读起来比较轻松，但能帮你建立对领域发展的整体认知。
- 如果时间充裕，再回到数学推导部分，把公式自己推一遍。
6.2 一些读完之后的个人体会
这篇论文我断断续续读了三遍，每一遍的感受都不一样。第一遍觉得内容太多、太散；第二遍开始意识到“增量式运动估计”这个定义是整个领域的锚点；第三遍带着实际项目中遇到的问题再读，才发现每一个看似简单的结论背后，都是从无数次实验失败里提炼出来的。
如果要我用一句话总结这篇综述的价值，我会说：它系统性地回答了一个核心问题——在单目、双目乃至多目配置下，如何把图像中的二维运动信息一步步转化为稳定的三维运动轨迹，以及这个过程在数学和工程上分别受制于什么。理解了这些，你再看各种新的VO/VIO/SLAM论文，会发现它们大多是在这条主线上做了局部增强。技术进步很快，但这套根基，值得你花时间夯实。
