本来没打算这么快写这篇笔记的。前段时间为了给一台移动底盘做视觉跟随我把视觉里程计Visual Odometry相关的资料翻了个底朝天最后落到一篇专门讲“前三十年与基础”的综述上Visual Odometry Part I: The First 30 Years and Fundamentals。它不算新却意外地比很多零散的博客和开源代码更值得精读。读完一个很直接的感受是——这项技术根本没有过时甚至今天各种SLAM框架里的核心思想仍然能从这篇综述里找到对应章节。这篇笔记适合两类人一类想系统入门视觉里程计与SLAM的人另一类是已经在用现成框架、但总被内部状态搞糊涂的工程师。我不会把论文复述一遍而是按“它想解决什么问题、用了什么数学工具、工程上怎么落地”来重新拆解顺便把我自己复现和实测中的一些判断也写进去。1. 综述的骨架一个定义、一条主线和一套基础1.1 视觉里程计的准确边界原综述对视觉里程计给出了非常朴素的定义利用相机获取的连续图像序列通过图像间的对应关系增量式地估计传感器自身运动的过程。这里的关键词是“增量式”——它不追求一次性恢复全局地图而是专注于相邻两帧或一小段图像窗口之间的相对位姿变化。这个定义把视觉里程计和运动恢复结构SFM区分开了。后者更关心离线地恢复一堆三维点和相机轨迹可以全局优化很多次而视觉里程计要在在线运行的情况下一帧一帧地算出“我往前走了多少、转了多少角度”。它不要求地图的绝对正确但要求运动增量足够平滑、足够准。在实际系统里视觉里程计很少单独存在。轮速计在小坡上会打滑惯性测量单元在长时间工作后会漂移而相机提供的视觉约束恰好能在短时间尺度内给出非常稳定的运动估计。所以视觉里程计最常见的角色是作为组合导航里的一个观测源和高频的惯性数据融合既保高频又保准头。1.2 一条主线从特征匹配到运动估计的流水线综述把视觉里程计的前端拆成了一条固定的流水线特征提取、特征匹配、外点剔除、运动估计、局部优化。这个流程今天看似乎平淡无奇但放在三十年尺度里它几乎定义了整个视觉导航领域的工程范式。特征提取从图像中挑出角点、斑点等稳定可重复检测的位置特征匹配建立两帧图像之间特征的对应关系外点剔除把匹配错误的点筛掉避免脏数据污染求解运动估计基于几何模型解算旋转和平移局部优化对最近一段轨迹和路标做集束调整压制累计误差。我读这篇综述时印象很深的一点是作者没有把“特征匹配”当作细节带过而是花了很大篇幅讲匹配质量对最终运动估计的影响。原因不复杂下游所有几何公式都假设输入是正确的匹配对一旦匹配错得离谱后续的一切数学工具都救不回来。很多初学者喜欢研究各种精巧的优化算法却忽略了让数据变干净才是性价比最高的一步。1.3 我理解中的“基础”是什么论文标题里的“Fundamentals”不是指“简单入门知识”而是指“支撑整个技术方向的底层定律”。视觉里程计之所以成立依赖三条底层事实刚体运动可以建模为旋转加平移针孔相机投影把三维空间映射到二维图像相邻两帧图像之间的时间间隔足够小运动变化有限。这三条事实看起来像是废话却是所有估计方法的地基。比如为什么要用特征点而不是直接用整幅图像因为特征点的位置可以亚像素级提取比整图匹配更稳定。为什么要用极线约束而不是直接三角化因为在单目相机下一次观测无法直接恢复深度。读这篇综述时每一章几乎都能落到这三条事实上这也是它作为“第一部分”最扎实的地方。2. 三十年演进从导航配角到SLAM基石2.1 上世纪80年代之前视觉导航的前史视觉里程计这个词在2004年前后才被广泛使用但相关尝试早得多。上世纪六七十年代计算机视觉研究者就开始做“运动分析”目的很接地气让机器人通过摄像头判断自己到底往哪走了。那个年代算力极其有限图像分辨率低存储更是奢侈品所以早期的做法通常是提取少数几个稳定特征比如墙角的角点、走廊里的亮点然后追踪这些特征在图像序列里的位移。这类早期系统的效果并不好但确立了一个重要观念视觉可以被当作一种“里程计”来用它不是用来识别物体的而是用来测量运动的。后来很多教科书把这个阶段叫作“导航辅助期”我觉得这个总结很到位因为那时候视觉在导航系统里只是配角主角是惯性传感器和轮式编码器。2.2 80到90年代地外探测与车载视觉点亮的第一个高峰视觉里程计真正作为独立技术分支被重视是在它被装上进行地外探测的行星车之后。原因非常现实地外表面没有GPS轮式里程计会因为地形打滑而失效惯性元件又会持续漂移而相机能够提供额外的运动约束特别是在轮子空转时依然能通过地面纹理判断是否前进。这类系统采用的思路在今天看来相当经典先对图像进行校正与去畸变在窗口内追踪特征点用立体视觉恢复三维点再通过帧到帧的配准估计运动。需要注意的是地外场景光照变化剧烈、地表纹理模糊、沙尘遮蔽频繁这些环境倒逼着算法在鲁棒性上不断进化。早期视觉里程计能在这样的条件下稳定工作反过来极大推动了地面机器人领域对这项技术的信任。到了90年代后期汽车辅助驾驶研究也开始出现视觉里程计的影子。车载场景的优势是纹理丰富、基线稳定但劣势同样突出动态车辆、行人、光照突变、运动模糊。综述里对“动态物体干扰”的讨论在今天仍然是实车上最头疼的问题之一。2.3 2000年代特征描述子与鲁棒估计的成熟进入二十一世纪后视觉里程计进入了方法论快速成熟的阶段。尺度不变特征变换SIFT这类描述子的出现让特征在尺度变化和旋转下依然可匹配随机抽样一致性RANSAC算法让系统能够在大量错误匹配中稳健地求出模型集束调整Bundle Adjustment被引入滑动窗口使局部轨迹的精度大幅提升。这个阶段还有一个容易被忽略的进步相机标定的标准化。标定棋盘格和开源标定工具链的成熟让研究者能把内参和畸变系数做得比较准而不再是凭经验手工估计。综述里用大量篇幅讨论标定误差如何传递到最终轨迹说明在方法论基本稳定后工程细节开始成为制约精度的主要瓶颈。2.4 2010年前后的分叉直接法登场综述写作时基于特征提取的稀疏方法已经是绝对主流但文本里已经清晰指出了另一条路直接利用图像像素亮度跳过特征提取和匹配直接最小化光度误差。这一派后来被统称为直接法它不再依赖特征点的可重复性而是利用整幅图像的信息在纹理缺失和运动模糊场景下往往表现得比特征法更顽强。直接法和特征法的路线之争其实是“保精度”与“保鲁棒”之间的权衡。特征法把数据压缩成了少量点计算干净但对纹理要求高直接法用密集信息换鲁棒性但计算量大、对光照变化敏感。今天主流的大型开源SLAM系统很多都吸收了这两种思路的混合设计。读综述看到这一节时会有一种“原来后面的故事在这里就埋下了伏笔”的感觉。3. 底层几何与投影约束读视觉里程计绕不开的三张图3.1 三个坐标系之间的换算视觉里程计的所有公式都建立在一套固定坐标系定义上。传感器运动由旋转矩阵R和平移向量t描述空间中的三维点则照相机内参投影到像素平面。最常见的投影公式可以写成z · p_uv K · (R · P_w t)其中p_uv是齐次像素坐标P_w是三维空间坐标K是相机内参矩阵z是点在相机坐标系下的深度。这个公式看起来简单却是所有后续推导的地基。初学者常犯的第一个错误就是坐标系定义混乱到底R是把世界系转到相机系还是把相机系转到世界系这一错平移方向直接反掉。我的习惯是每次实现都先画一张坐标系的图把“左乘还是右乘、作用于谁”明确写下来再动手写代码能省下大量调试时间。3.2 针孔模型、畸变与标定参数图像上的特征坐标对应的是像素位置但实际投影模型里除了焦距和主点偏移还存在镜头畸变。综述特别强调标定结果的准确性会直接影响视觉里程计精度因为整个数学链路上所有输入都会被投影方程放大。举个例子如果畸变参数偏差了几个像素边缘区域的特征点位置会被系统性带偏这种误差不会像随机噪声那样在优化中被平均掉反而会在连续多帧中累积成轨迹的弯曲。我在一次模拟项目X的测试里故意把标定焦距调偏1%结果十米直线轨迹跑到约九十米处偏出了将近半米。事后复盘问题根源并不在运动估计算法而是标定参数。3.3 数据关联比运动估计更容易被低估的一环给定两帧图像实际上先得知道哪些像素点是同一个空间点的投影。这依赖特征匹配或光流跟踪。综述把下游所有数学约束都建立在“已给出对应关系”的假设上但在现实里如何高效、稳健地获得对应关系才是工程难点。特征匹配的策略选择非常影响整体性能。暴力匹配在特征数量达到上千时就会显得吃力所以工程上常用快速最近邻搜索而序列图像之间运动变化不大时基于金字塔光流的特征跟踪比每帧重新匹配要快得多。我的经验是在车载和机器人场景里先用稀疏光流跟踪上一帧特征只在跟踪丢失较多时补充新特征并重匹配这样可以同时保证速度和稳定性。4. 三种运动估计路线2D-2D、3D-2D、3D-3D4.1 2D-2D对极几何与五点法当只有两帧图像中匹配的二维点、没有三维信息时只能借助对极几何。在齐次坐标下两帧匹配点p1、p2满足约束p2^T · F · p1 0其中F是基础矩阵。如果相机已经标定则可以进一步用本质矩阵E来表示E t^∧ · R其中t^∧是平移向量的反对称矩阵。本质矩阵E包含了旋转和平移信息但平移方向只能恢复到尺度意义下的值——这正是单目视觉里程计“无尺度”问题的根源。从本质矩阵分解出R和t并不直观通常有四种数学可行解需要借助三角化来检验哪个解把三维点放在了两个相机前方才确定唯一解。这就是为什么单目视觉里程计必须先初始化一开始没有三维点只能靠2D-2D的相对运动估计建立初始结构之后的帧才能转入三维观测模式。4.2 3D-2DPnP与已知路标下的定位当空间中的三维点与图像二维点的对应关系已知时问题变成求解相机在当前帧的位姿。这就是透视n点问题PnP。一类经典做法是直接线性变换DLT通过构建线性方程来解位姿另一类是基于迭代优化的做法更快的同时对噪声也更稳健。PnP模式在双目与深度相机视觉里程计里成了主力。拿双目来说左右图像可以直接三角化出三维点后续帧与这些点建立3D-2D对应每一帧都通过PnP解算运动而不再需要每帧都做对极分解。这样做的好处是鲁棒性高、平移尺度天然确定也是工程上我更推荐的方式。单目系统在初始化完成之后同样可以转入PnP模式因为此时已经有了早期三角化得到的路标点。4.3 3D-3D迭代最近点ICP配准如果连续两帧都有三维点比如深度相机直接输出像素对齐的深度图那么问题更进一步简化为两组三维点集合之间的刚体变换求解。经典做法是迭代最近点ICP核心思想是反复寻找两个点集之间的最近匹配并更新变换直至收敛。ICP的缺点是容易陷入局部最优对初始变换估计敏感。工程上通常先给一个粗略运动预测比如运动模型插值或IMU积分结果再用ICP做精配准。在深度相机场景中由于观测质量受距离和材质影响点云本身存在大量外点因此采用带鲁棒核的ICP或先做点云下采样往往比直接做精配准效果更好。4.4 一张表概括三种运动估计路线路线输入关联核心约束典型传感器适用阶段2D-2D像素到像素对极几何约束单目相机初始化、无深度信息的早期帧3D-2D地图点到像素透视投影重投影双目、深度、单目初始化后全局定位与局部运动估计的主力3D-3D点到点刚体变换一致性深度相机、激光雷达融合密集点云配准三种路线在同一个系统里经常交替使用。比如单目系统起步时用2D-2D初始化随后转为3D-2D跟踪当滑动窗口内需要精修时又可能用3D-3D或重投影误差统一优化。理解这三种姿势之后视觉里程计的前端就不再是一堆神秘黑盒而是一套可被拆解和组合的几何工具。5. 鲁棒与精度视觉里程计真正难在失控之前5.1 RANSAC为什么能救场特征匹配输出的大量对应点里总有一部分是错误匹配。对最小二乘类求解器来说一个外点的破坏力远大于一百个内点的贡献因为平方误差会让离群项获得过大的权重。随机抽样一致性RANSAC用“多次采样假设检验”的思路每次取少量点求模型再用全部数据检验该模型的共识程度最终保留支持度最高的模型。这个机制并不复杂关键是参数设置。迭代次数决定模型被找到的概率内点阈值决定“多远算内点”。阈值设得太小系统会频繁报退化阈值设得太大外点混进来、精度下降。我在实践中通常把阈值设在1到3像素之间并先看匹配对的基础矩阵检验值分布再决定。视觉里程计的每一个环节都值得先可视化确认而不是直接跑整体结果。5.2 局部优化与全局一致的边界累计误差是视觉里程计的天性无法消除只能缓解。综述对漂移的解释非常清楚每一帧的运动增量都带有微小误差这些误差在积分过程中不断叠加导致轨迹相对真实路径慢慢漂移。为了把漂移压住工程上常用两类手段滑动窗口局部优化和关键帧机制。滑动窗口保留最近若干帧和可见路标点一起做集束调整让局部轨迹互相约束关键帧机制只保留信息量大的帧参与优化防止冗余帧拖慢计算。单目视觉里程计还有一个隐藏的漂移来源累计尺度的缓慢变化。由于单目观测到的平移量天然无尺度任何一点尺度估计偏差都会随着运动被放大这也是单目系统长时间运行时轨迹经常“越走越近”或“越走越远”的原因。要想根治只能引入双目、深度传感器或与惯导融合。5.3 退化场景什么时候视觉里程计会直觉性失效我复现过程中最有价值的经验来自对退化场景的识别而不是对算法的调整。纯旋转相机原地转动、没有平移时对极几何中的平移约束退化单目系统无法可靠估计旋转幅度与场景结构纹理缺失白墙、雪地、大片天空特征点数量不足匹配直接失败重复纹理格栅、条纹里的匹配容易错位到周期对应的位置视觉里程计可能给出跳跃感明显的轨迹动态物体行人、车辆遮挡大块视野时匹配点会被带偏光照突变突然逆光或进入阴影特征描述子匹配率会断崖式下降。这些场景不能靠调参硬撑。成熟系统里状态机会根据影像质量、内点比例、速度合理性等指标判断当前是否可信一旦置信度不足就切到其他传感器或暂停里程积分。把退化情况写进系统设计里比事后做花式补救省太多人力。6. 读完综述之后我的工程落地节奏6.1 先跑通无尺度单目再切双目与深度很多初学者一上来就想复现完整SLAM结果被一堆模块嵌套搞得举步维艰。我的建议是严格按视觉里程计的流水线走从相机标定开始拿到准确的内参、畸变与投影矩阵用两帧图像做一个最简2D-2D运动估计画出本质矩阵分解后的轨迹加入特征匹配与RANSAC比较外点比例对轨迹的影响实现三角化把单目初始化做完再转到3D-2D模式加入滑动窗口与局部集束调整观察漂移如何被压制如果传感器支持直接切换双目或深度相机对比精度与稳定性。每一步都先追求“能跑、能可视化”再考虑效率。我自己的项目就是这么过来的前两个步骤花了最长时间但后面换任何传感器和框架都变得很快。6.2 效率指标与验收清单在工程里不能只说“看起来跑得还行”要给指标。我常用的验收清单包括指标说明可接受范围普通室内/园区场景轨迹精度ATE估计轨迹与真值的全局差异平移误差小于总里程的1%相对位姿误差RPE固定间隔位姿增量的误差越小越好观察均值与方差特征匹配成功率经RANSAC剔除后的内点占比大于50%算健康单帧处理耗时在目标平台上的运行时间视帧率要求而定一般小于帧间隔一半退化触发率状态机判定不可信的时间占比常规场景低于1%调试时不要只看最终轨迹图要分层看数据匹配连线的可视化、内点分布、每帧的运动增量、协方差估计。发现问题时逐层排查比在黑盒里反复调参数高效得多。6.3 三句话留给初学者第一调试时别只盯着轨迹先检查特征匹配质量与外点比例。匹配质量一旦崩了后面所有模块都是垃圾进垃圾出。第二标定多做几次不吃亏。重新标定半小时能省掉后面无数个怀疑人生的晚上内参和畸变不对轨迹往往在拐弯处开始不可逆地偏。第三把退化场景写进状态机。诚实承认“我不知道自己在哪”比硬报一个错误的位姿安全得多这也是视觉里程计系统走向成熟的关键一步。7. 这篇综述留在现代系统里的印记7.1 特征法与直接法并不是敌人现代大型SLAM系统里很多都采用了“混合”设计用直接法或半直接法估算帧间运动摆脱特征提取的耗时与纹理依赖同时维护一张稀疏或半稠密的地图再用特征或关键帧做全局匹配。这套组合的根依然能回到综述里对视觉里程计前端流水线的描述。后端的变化更大。现在的主流系统普遍引入因子图优化、滑动窗口边缘化、惯性预积分等技术这些基本都是视觉里程计“局部优化”的扩展。可以说视觉里程计在SLAM里的角色就像一个“快速但短视”的前端观测器它先给出里程增量的种子再由后端把这些种子放到更大尺度的优化框架里。7.2 未来的方向在综述里已现端倪学习型的视觉里程计在近些年开始增多直接让网络从图像序列回归运动增量。这类方法的优势是能够在光照变化和纹理缺失场景下保持鲁棒劣势是泛化能力与可解释性有限。有趣的是这些方法大多仍然结合了某种几何约束作为监督信号或者与经典视觉里程计融合做后端校验。传感器融合是另一个明显趋势。把相机与惯性测量单元做紧耦合利用相机标定尺度、抑制陀螺仪零漂同时用高频惯性数据补偿相机的运动模糊这种互补关系让系统既快又准。读综述时“这个方向当时还是展望”的诸多段落今天几乎都变成了产品级的工程技术。7.3 我的私人观察基础越扎实后续越敢换花样这几年我把视觉里程计相关代码重写过三遍每一遍都会发现新的理解。第一遍读这篇综述时我对“增量式估计”“局部优化”等概念只是记住了第二遍做双目视觉里程计时才真正理解3D-2D模式为什么比2D-2D好用第三遍融合惯性数据时才完全看懂滑动窗口边缘化为什么能把尺度约束锁住。所以这篇“Part I”最珍贵的地方不是某个新鲜点子而是它把视觉里程计的整个问题空间摊开了定义、历史、输入输出、数学模型、工程流程、退化边界。无论后面选择做特征法、直接法、学习法还是深度融合要解决的核心问题都没有跳出这套框架。我自己在这篇综述里翻到最多折痕的不是某个新算法而是关于漂移和退化场景的那几段。做视觉里程计蛮容易产生“多加点优化就能无敌”的错觉但真正让系统长时间稳定的是对约束边界的清醒认识。如果只留一条建议把相机标定、数据关联、鲁棒估计三个环节按论文里的顺序各写一个最小可运行实现后面再看任何视觉SLAM论文都会轻松很多。