ORB-SLAM2源码解析：从main函数到多线程协同的完整运行流程

刚开始啃ORB-SLAM2源码的时候我相信绝大多数人跟我一样第一反应就是找到main函数然后顺着调用的函数一路追下去。结果呢追到System构造函数就懵了因为程序压根不是一条直线往下跑的——它开了一堆线程各干各的活还通过共享的数据结构互相拉扯。这篇是我自己梳理过的代码运行流程笔记默认你已经能跑通Demo、也大致知道Tracking、LocalMapping、LoopClosing这三个模块是干嘛的接下来就是要把这三个模块在代码里的真实运行顺序、线程间怎么交接、单目双目RGB-D三种模式差在哪全部串起来。看完你应该能回答程序到底从哪一行开始、每一帧图像进来之后发生了什么、三个线程什么时候被唤醒、关键帧和地图点是怎么流动的。如果只想要一个结论那就是ORB-SLAM2的代码主线是“图像帧→Tracking→关键帧→LocalMapping→局部地图→LoopClosing→全局地图”但这根线是异步的三条线程之间靠队列和标志位耦合。下面按实际执行顺序拆开讲。1. 代码整体架构与多线程模型1.1 三大核心线程的划分ORB-SLAM2整个系统的核心逻辑全部集中在System类里。System的构造函数会依次做几件大事加载ORB词典、读取相机参数、创建三个核心对象Tracking、LocalMapping、LoopClosing然后显式创建三个线程并启动它们。这三个线程的入口函数分别是Tracking::Run()、LocalMapping::Run()、LoopClosing::Run()。如果开启了GUI还会额外启动一个Viewer线程不过它对整个算法主流程没有影响可以暂时忽略。这三个线程的分工非常清晰Tracking线程处理每一帧输入图像负责特征提取、位姿估计、局部地图跟踪以及判断是否产生关键帧。LocalMapping线程消费Tracking产生的新关键帧做局部地图点的创建、筛选和局部BA优化。LoopClosing线程检测回环做回环修正和全局BA。为什么要拆成三个线程而不是一个主循环轮流调用核心原因是三者的实时性要求完全不同。Tracking必须跟着相机帧率走30帧每秒就是30帧每秒不能因为局部优化没做完就卡住下一帧。LocalMapping可以慢一点只要在一两帧关键帧的间隔内消化完就行。LoopClosing更不着急可能隔几秒才触发一次但它一旦触发就需要做全局计算。如果全塞在一个线程里Tracking会被拖死系统实时性就没了。1.2 三类传感器的统一入口与差异System构造函数的第二个参数是传感器模式代码里用枚举区分MONOCULAR、STEREO、RGBD。这三种模式在System层面对应的入口函数分别是TrackMonocular()、TrackStereo()、TrackRGBD()输入参数分别是图像和时间戳双目和RGBD会额外传入深度信息。一个经常被忽略的点是虽然有三个不同的入口函数但它们的核心逻辑都收敛到同一条处理链路区别主要在于输入预处理和深度图的处理方式。单目相机没有深度信息只能靠多视图三角化恢复尺度所以初始化必须等到第二帧而且对两帧之间的视差有要求。双目相机可以直接通过左右目匹配算视差每一帧都能得到稀疏深度。RGBD更简单直接读深度图但需要处理深度值单位缩放和无效像素。代码里这三个入口最终都会调用Tracking::Track()而Track()内部会根据传感器类型走不同的分支。理解这一点很重要因为很多报错和参数配置问题根源就在“模式选错”或者“深度数据没读对”上。2. 系统初始化与主入口2.1 main函数的行为拿最常见的公开SLAM数据集的例子来说可执行文件启动后argv[1]是图像关联文件的路径argv[2]是相机参数配置文件。main函数的流程是读入相机参数配置文件路径创建System对象然后逐行读取图像序列调用系统接口把图像和时间戳传入最后在图像处理完后调用保存轨迹的接口。创建System对象的开销比很多人想象中要大。构造函数里最耗时的部分是加载ORB词典这个词典文件包含了海量ORB描述子的聚类树加载过程要花好几秒。词典是回环检测和重定位的基础加载失败或者版本不对后面运行起来基本就是各种零匹配。main函数里图像循环那段代码很简单本质上就是读取一张图、调用一次TrackMonocular、存下时间戳和位姿。但有一个细节值得注意轨迹保存时时间是按固定小数位输出的这是为了和数据集提供的地面真值轨迹做对齐评估如果你写自己的工具脚本格式最好保持一致。2.2 相机参数与配置文件相机参数配置是通过yaml文件传入的。文件里最关键的是内参fx、fy、cx、cy畸变系数以及图像分辨率。System构造时会用这些参数去构造相机模型和ORB提取器。如果内参写错了位姿估计结果会整体漂移甚至在初始化阶段就卡住。RGBD模式还会用到深度图缩放因子。很多深度相机的深度图不是直接以米为单位的原始值乘以一个缩放系数才能得到米。这个系数如果配错地图的尺度看起来会对但实际上深度误差大得离谱回环检测也容易出问题。我调试时发现这个参数是RGBD模式最容易踩的坑之一而且它在代码里不会报任何错误地图照常构建只是后续的轨迹误差也明显偏大。单目模式下ORB-SLAM2要求图像是灰度图。代码内部会用OpenCV做颜色转换如果你的数据源是RGB顺序的彩色图但yaml里配置成了BGR出来的灰度图不会错但后续的显示和特征描述符匹配会变得很怪。正确的做法是确认好自己数据源的颜色通道顺序再在yaml里设置对应的RGB标志。2.3 单目初始化流程单目初始化是整个系统中最让人困惑的部分因为它在代码里不是简单的一次性调用。Tracking::MonocularInitialization()会维护一个存放初始帧的成员变量当它收到第一帧时直接拷贝并返回收到第二帧时才把两帧的特征点做匹配然后用对极几何恢复初始位姿。初始化过程同时计算了单应矩阵H和基础矩阵F这是ORB-SLAM2的一个特色。它根据匹配点数量选择更合适的模型如果场景近似平面H矩阵更稳定如果场景比较立体F矩阵更合理。得到矩阵后通过SVD分解出旋转和平移然后三角化出初始地图点。初始化对运动条件要求苛刻两帧之间必须有足够的视差但又不能大到特征跟踪丢失相机需要有一定的平移纯旋转的情况下无法三角化。这就是为什么对着白墙或者静止场景启动系统会一直卡在初始化阶段控制台反复提示尝试初始化。新手遇到这种情况不要急着调代码先让相机左右缓慢平移积累视差。3. Tracking线程的逐帧处理过程3.1 特征提取与匹配的代码主线Tracking::Track()是每一帧图像的核心处理函数它的第一步是提取ORB特征。ORB提取器在系统构造时就已经创建好了参数包括特征点数量上限、金字塔缩放因子、金字塔层数还有FAST角点检测的初始阈值和最小阈值。特征提取的参数直接决定了整个系统的速度。特征点数量上限越高提取和匹配耗时越长金字塔层数越多尺度变化鲁棒性越强但内存占用和计算量也随之上升。我自己的经验是在室内低纹理场景特征点阈值可以适当调低让系统提取到更多角点在纹理丰富的室外场景阈值调高一点反而更稳因为低阈值的角点大量集中在树叶、草丛这些噪声区域对位姿估计帮助不大。特征提取完成之后Tracking会尝试把当前帧的特征点和地图点对应起来。这里有个常见的误解不是所有地图点都要匹配而是先通过参考关键帧或运动模型预测它们在当前帧的大概位置然后只在预测位置附近搜索。这个“先预测、再搜索”的策略就是ORB-SLAM2前端高效的关键。3.2 位姿估计的三种模式切换Tracking::Track()的核心是一个switch分支结构根据当前系统的跟踪状态决定用哪种方式估计位姿如果上一帧跟踪正常当前帧优先使用运动模型假设相机匀速运动用上一帧位姿和速度预测当前帧位姿然后把地图点投影到当前帧在投影点附近搜索匹配。如果运动模型跟踪失败或者当前帧是重定位之后的第一帧就改用参考关键帧模型直接用参考关键帧的Bow向量做匹配然后用PnP求解位姿。如果前面两种都不行就进入重定位模式。重定位会查询整个地图的词袋索引找到若干候选关键帧再逐一用PnP求解成功后再做局部优化。这三种模式的切换逻辑看起来不复杂但代码里包含了大量的阈值判断和标志位管理。我读源码时建议先只看分支的进入条件不要过早陷入匹配细节否则会迷失在特征点数组和描述子矩阵里。3.3 关键帧判定与局部地图跟踪Tracking线程在每一帧跟踪完之后还要决定是否把当前帧提升为关键帧。这个决策由NeedNewKeyFrame()函数完成里面判断了几个条件局部建图线程是否空闲、距离上一个关键帧的帧数是否超过阈值、当前帧跟踪到的地图点数量是否足够、当前帧和参考关键帧之间的视差是否足够大。这些条件的组合逻辑很直观如果局部建图线程很忙就不要硬塞新关键帧否则队列会积压如果跟踪质量很好说明当前帧信息量和已有地图重叠度高也不必急着建关键帧如果视差足够大说明相机看到了新的场景内容值得生成关键帧。关键帧判定通过之后还有一个重要步骤叫TrackLocalMap()。这一步的本质是把当前帧和局部地图中的关键帧、地图点做一次完整的匹配和联合位姿优化。前面几步只是初步估计位姿局部地图跟踪才是把当前帧真正“钉”在地图上的步骤。代码在这里会统计当前帧能观测到的局部地图点数量如果这个数量小于一定阈值就认为跟踪丢失进入重定位流程。4. LocalMapping线程关键帧与地图的消化流程4.1 新关键帧队列与处理顺序Tracking线程判定关键帧后会调用mpLocalMapper-InsertKeyFrame()把当前关键帧放进LocalMapping线程的待处理队列里。LocalMapping::Run()则在一个循环里持续检查这个队列一旦有新的关键帧就依次执行四步操作处理新关键帧、筛选地图点、创建新的地图点、局部BA优化。这四步是有严格顺序依赖的。第一步做的是把新关键帧的信息注册到地图里更新它与既有关键帧、地图点之间的观测关系。这一步如果没做完后面的地图点匹配和三角化都无从谈起。第二步是地图点筛选这一步很“暴力”如果一个地图点在创建后的头几帧内观测次数过少就直接删除。这个策略是为了防止那些由误匹配或者三角化质量极差的地图点长期留在地图里它们会严重干扰后续的帧间匹配和优化。第三步创建新的地图点是通过当前关键帧和它附近共视程度最高的若干关键帧做极线匹配然后三角化出新一批地图点。这里代码会检查两帧之间的视差视差太小三角化得出的深度不可靠视差太大极线搜索的匹配范围又过大。实际调参时要留意这一步的日志如果创建的地图点数量长期为0多半是极线搜索的阈值设得太苛刻了。4.2 局部地图维护中的代码细节局部地图维护最核心的类是一个MapPoint。MapPoint对象里不只存了世界坐标还维护了一个被观测的“名单”哪些关键帧看到了它、在关键帧的哪个特征点上、描述子是什么。每一次局部BA或者回环修正都会更新这些关联关系。代码里MapPoint的成员变量很多读的时候会觉得很乱但抓清“世界坐标、描述子、观测关键帧列表”这三样基本上框架就清楚了。MapPointCulling这一步的代码尤其值得细看。它判断一个地图点是否需要被剔除时会看两个计数一个是该地图点被多少个关键帧观测到另一个是它在最近几帧内是否连续可见。如果观测次数少又连续多帧没在地图中被匹配上它就会被标记为bad紧接着在下一次遍历中被清除。这种机制保证了地图里留下的点都是“有生命力”的避免被大量离群点污染。4.3 局部BA的优化范围与边界处理LocalMapping第四步调用局部BA优化当前关键帧和它周边的共视关键帧以及这些关键帧能看到的地图点。局部BA的理论基础是图优化代码里用g2o库来构建误差项。优化过程中所有参与优化的关键帧位姿和地图点坐标都会作为变量更新但不在当前局部窗口内的关键帧只提供约束不参与更新。这一步的耗时和局部窗口大小直接相关。机器性能不够的时候局部BA会让LocalMapping线程的CPU占用率居高不下但Tracking线程仍然保持实时。这也是把LocalMapping拆成独立线程最大的收益。代码里还有一个设置值得关注SetAcceptKeyFrames()它会让Tracking线程在建图繁忙时暂停插入新关键帧避免队列无限膨胀。5. LoopClosing线程回环检测与全局优化流程5.1 回环检测与候选关键帧筛选LoopClosing线程和前两个线程不太一样它大多数时间都在“睡觉”只有等到关键帧队列里有东西才醒过来。检测回环的第一步是提取当前关键帧的BoW向量然后在关键帧数据库里查询相似度超过阈值的关键帧。这一步利用了ORB词典和DBoW2库查询的效率很高不用和所有关键帧做暴力描述子匹配。查询出来的候选关键帧会经过一层“共视几何校验”。代码要求候选关键帧和当前关键帧之间要有足够多共视的地图点同时还要满足连续性校验避免单帧的偶然高相似度造成误检。这层校验通过之后才会进入真正的几何验证环节。5.2 Sim3求解与回环修正回环检测的几何验证使用的不是普通的位姿变换而是带尺度的Sim3变换。为什么必须是Sim3因为在单目SLAM中地图的尺度是漂移的系统在回环处的真实误差里包含了尺度不一致。Sim3变换描述的正是三维空间的相似变换包含旋转、平移和尺度因子三项能统一修正视觉里程计的尺度漂移。代码里Sim3的求解由RANSAC流程包裹。先通过描述子匹配找到当前关键帧和候选关键帧的匹配点对然后随机采样求解Sim3再用内点数量评估模型质量。求解成功后LoopClosing会把当前关键帧以及它周边的关键帧一并修正到回环位置这个动作的代码实现集中在CorrectLoop()里。CorrectLoop()是整份源码里最复杂、最容易出错的函数之一。它要同时调整多个关键帧的位姿和大量地图点的坐标而且调整的先后顺序绝对不能乱。我看到过不少二开项目在这里翻车原因基本都是在修改地图点时没有同步更新KeyFrame里的特征点信息导致后续匹配错位。如果你要在这份代码上做改动这个函数一定要吃透。回环修正完成之后系统会做一次Essential Graph优化把修正沿共视图传递到相连接的帧上让整个地图保持一致。5.3 全局BA的触发时机与结束处理回环修正之后LoopClosing线程会启动全局BA。全局BA会优化地图里所有的关键帧和地图点是整份代码里计算量最大的一次优化。它的执行被放在一个单独的线程里所以不会完全阻塞回环检测线程。全局BA结束后最重要的一件事是“更新传播”。因为全局BA期间新的关键帧依然可能产生并插入到系统中这些新关键帧还没有参与全局优化它们的位姿还是旧的。代码会把全局BA优化完的关键帧位姿、地图点坐标重新写回再把这些改动传播到新的关键帧上。这个过程没有在图论上做到绝对完美但对于实际建图来说已经足够。很多人在看代码时会疑惑为什么全局BA都做完了还要再更新一轮关键帧位姿原因就在这里。6. 关键类与数据结构的联动关系6.1 Map、KeyFrame、MapPoint三者的关系Map类是整个地图数据的容器它内部维护两个列表所有关键帧的列表和所有地图点的列表。Map提供的操作无非是插入、删除、查询但它的地位很特殊因为三个线程都要访问地图读多写少代码里用互斥锁保护关键区域。KeyFrame和MapPoint之间存在双向关联。KeyFrame里存储了它看到的所有地图点的指针以及每个地图点对应的特征索引和描述子索引。反过来MapPoint里也存储了哪些KeyFrame观测了它以及观测时对应的特征点索引。这种双向指针结构让代码在任意一端都能快速找到另一端但也带来了一个隐患任何一个指针更新漏了整个地图数据就处于不一致状态。这也是ORB-SLAM2代码里最容易出现悬空指针的地方。6.2 Frame与KeyFrame的区别Frame是一个轻量级的临时对象代表当前正在处理的图像帧。它保存特征点、描述子、位姿估计结果以及一些金字塔层级信息。Tracking线程每一帧输入都会创建一个Frame处理完之后这个对象要么被丢弃要么被转成KeyFrame。KeyFrame是地图里的“常住居民”它从Frame切换过来的瞬间会额外生成很多数据BoW向量、共视关键帧列表、生成树关系、参考关键帧指针。Frame进化为KeyFrame时代码会调用UpdateConnections()来计算共视关系这是后面局部BA和回环检测的图结构基础。对初学者来说最直观的区别是生命周期Frame是流水的KeyFrame是铁打的。调试时看到位姿一直在变先想清楚你打印的是Frame还是KeyFrame的位姿两者在同一个时刻的值可能完全不同。6.3 ORB词典与DBoW2的作用DBoW2库负责把ORB描述子转换成单词向量。具体来说词典文件里预定义了海量描述子的聚类中心每个特征描述子都可以被归到最近的聚类中心这个聚类中心就是一个“单词”。一帧图像提取了若干个特征点每个特征点对应一个单词统计每个单词出现的次数就构成一个稀疏的BoW向量。BoW向量最直接的用处是快速检索比较两帧是否相似不用逐一匹配描述子只要计算BoW向量的相似度分数就行。回环检测和重定位的第一步都是靠它缩小候选范围。实际使用中如果你的词典和提取特征的参数差异过大比如特征点类型改了或者描述子维度变了BoW查询结果就是一堆零匹配现象就是重定位永远失败或者回环永远检测不到。遇到这种情况优先检查词典文件版本是否匹配而不是怀疑回环阈值设错了。7. 常见错误与调试经验7.1 编译与运行时的经典报错ORB-SLAM2的源码基于较早期版本的OpenCV编写用新版OpenCV编译会有不少接口变动问题最常见的报错集中在特征描述子类型和图像转换函数上。编译阶段如果报出大量fatal error多半是头文件路径不匹配。建议先在一个干净的虚拟环境里固定好依赖版本不要每次都在系统环境里动全局依赖。运行时最容易遇到的崩溃是段错误来源绝大多数是空指针要么是MapPoint已经被删除了但某个KeyFrame还保存着指向它的指针要么是词典加载失败导致BoW向量为空但后续代码没有判空就直接使用。调试这种崩溃优先在崩溃点往上游看凡是取出指针之后、使用指针之前的代码路径检查是否为nullptr。7.2 单目初始化失败与跟踪丢失单目初始化失败的现象是控制台不断出现初始化提示地图始终无法建立。排除配置问题后最常见的原因是输入图像质量太差画面模糊、纯纹理缺失、或运动过程几乎纯旋转。解决办法是把相机从原地“甩动”改成缓慢平移同时适当降低ORB的FAST阈值让系统能提取到更多特征点。跟踪丢失则发生在系统已经初始化成功之后表现为画面中当前帧的位姿估计失效系统转入重定位。重定位成功的概率受制于当前帧与地图的重复度。如果丢失后的画面是在一片新区域系统会长时间无法恢复。双目和RGBD模式相对好办因为它们每一个新帧都能直接生成深度信息可以快速构建新地图点而单目在丢失后只能依赖重定位。7.3 性能调优的实战心得ORB-SLAM2本身设计就能在CPU上实时运行但很多人把分辨率开到1080p之上导致特征提取成为瓶颈。我的建议是输入分辨率保持在640x480左右这是速度和鲁棒性的平衡点。如果精度要求高、计算力足够可以用更高分辨率但要接受帧率下降。特征点数量也是一个关键参数。默认值是每帧两千左右对普通场景完全够用降到一千能显著降低匹配耗时。局部BA的耗时也可以通过调整内参和优化迭代次数来控制但我不建议一上来就乱砍迭代次数会导致轨迹精度下降。更稳妥的做法是先用原参数跑一遍正常数据集记录每个环节的耗时再针对性调优。8. 从读懂到改动的个人体会把ORB-SLAM2代码完整读一遍和从零开始自己写SLAM是两种完全不同的体验。读源码最大的收获不是背下某个函数而是理解系统为何要这样分层Tracking只管前端实时性LocalMapping保证地图质量LoopClosing负责全局一致性。这种架构哪怕你后面换成深度学习特征或者加入语义信息这些模块边界依然是值得保留的。我自己改动这套代码时最先做的事情是给每个函数的入口和出口加了日志把关键帧编号、地图点数量、耗时打印出来。这套日志在后期定位问题的时候帮了非常大的忙因为三个线程并发跑光靠断点是很难看清数据流动顺序的。如果你打算在这个框架上做二次开发强烈建议先做到这一步。最后说一个最容易被忽略的细节当你修改了KeyFrame的位姿或者MapPoint的坐标之后一定要想一想还有哪些地方缓存了这个旧值。ORB-SLAM2的代码里这类缓存更新不及时导致的地图散架是绝大多数二开事故的源头。看清楚这些再往后加自己的模块心里就有底了。