1. 从一次地图漂移说起为什么要搞懂这四张“图”去年帮一个做室内巡检机器人的团队排查定位问题时遇到一个很典型的现象机器人在走廊里跑前两百米轨迹很稳一拐进回环区域地图就整体歪掉重定位也时灵时不灵。抓了日志一看局部跟踪没丢问题出在后端优化——回环检测明明触发了但位姿图优化之后误差并没有被真正摊平。后来把关键帧、共视图、扩展树、本质图这四者的关系重新捋了一遍才发现是共视图的权重设置和本质图的边选择出了问题导致优化时约束给错了地方。这件事让我意识到很多人跑 ORB-SLAM2 能跑通但一旦要改后端、调回环、做地图复用就会卡住。卡住的根源往往不是代码看不懂而是没搞清楚系统内部同时维护着好几套“图结构”它们各管一摊又互相喂数据。关键帧是节点共视图管局部优化扩展树管全局优化和回环本质图管位姿图优化和重定位。这四者不是并列关系而是层层递进、互相约束的关系。这篇梳理适合三类人看一是刚跑通 ORB-SLAM2、想深入后端但被各种图绕晕的开发者二是做 SLAM 二次开发、需要改共视图权重或本质图边策略的工程师三是做多传感器融合、想把视觉约束和别的约束接进这套框架的人。我会从设计动机讲起把每张图“为什么存在、存了什么、怎么用、什么时候会出问题”讲透再给一套可以直接对照代码复现的实操流程和排查表。你不需要先精通图优化但需要知道位姿、观测、约束这几个基本概念。2. 四张图到底各自解决什么问题2.1 先建立一个直觉SLAM 后端本质是在解一个超大稀疏图把 SLAM 后端想象成一张巨大的关系网每个关键帧是一个节点节点之间如果有共同观测到的地图点就拉一条边边的强弱取决于共同观测的数量。理论上所有关键帧两两之间都可以连边但那样图会稠密到没法算。所以工程上必须做取舍哪些边保留、哪些边丢掉、哪些边只在特定场景用。ORB-SLAM2 的聪明之处在于它没有用一张图打天下而是维护了四张不同稀疏程度、不同用途的图分别服务不同的优化任务。这就像公司管理共视图是部门内部的协作网扩展树是汇报线本质图是跨部门项目组关键帧是员工档案。你不可能用汇报线去管日常协作也不可能用项目组去替代汇报线。搞混了优化就会出问题。2.2 关键帧不是普通帧是“被选中的记忆节点”关键帧KeyFrame是这四张图的基础节点。它不是随便一帧图像而是经过筛选、被认为“值得记住”的帧。ORB-SLAM2 插入关键帧的条件大致是距离上一关键帧超过一定帧数、跟踪到的地图点少于阈值、或者局部建图线程空闲。这些条件背后是一个权衡插太多后端优化负担爆炸插太少观测约束不够轨迹会飘。每个关键帧存的东西很多位姿Tcw、相机内参、ORB 特征点、描述子、对应的地图点、共视关系、父子关系等。你可以把它理解成一个“压缩后的场景快照”。后面三张图全都是围绕关键帧之间的关系建立的。注意关键帧的位姿在局部 BA 和全局 BA 中都会被优化但它在扩展树中的父子关系一旦建立通常不会因为位姿优化而改变。这个细节在排查回环问题时很关键。2.3 共视图局部优化的“邻里关系网”共视图Covisibility Graph回答的问题是哪些关键帧和我“看得见同一片场景”如果两个关键帧共同观测到的地图点数量超过阈值ORB-SLAM2 默认是 15它们之间就有一条边边的权重就是共同观测数。共视图的核心用途是局部建图线程里的局部 BA。当新关键帧插入后系统不会优化整张地图而是只取该关键帧及其共视邻居通常是一阶邻居加上部分二阶组成一个局部窗口在这个窗口内做 BA。这样既保证了局部一致性又控制了计算量。共视图的边是动态的随着地图点被新增、融合、剔除共同观测数会变边权重也会变。但 ORB-SLAM2 并不会每帧都更新整张共视图而是在局部建图线程里按需更新。这里有个容易踩的坑如果你在外部改了地图点却没有触发共视图更新局部 BA 就会用旧的约束导致优化结果和实际观测不一致。2.4 扩展树全局优化的“骨架”和回环的起点扩展树Spanning Tree是从共视图里抽出来的一棵树它保留了共视关系中最强的那些边保证整张图连通且无环。每个关键帧在树里有一个父节点和若干子节点。扩展树的主要用途有两个一是全局 BA 时作为初始位姿传播的骨架二是回环检测时用来找候选关键帧。为什么需要扩展树因为共视图可能有环而很多算法比如位姿传播、回环候选搜索在树上做更简单、更稳定。扩展树相当于把共视图“骨架化”丢掉冗余边保留最强连接。回环检测时系统会沿着扩展树找与当前关键帧有共同单词但距离较远的候选然后再用共视图做几何验证。提示扩展树一旦建立父子关系通常固定。但如果在局部建图中某个关键帧被剔除扩展树需要做相应的父子重连这个逻辑在 ORB-SLAM2 里处理得比较隐晦改代码时要特别小心。2.5 本质图位姿图优化的“精简版共视图”本质图Essential Graph是共视图的进一步精简它只保留那些“必要”的边扩展树的边、共视权重高的边、以及回环边。本质图的作用是位姿图优化Pose Graph Optimization也就是在回环检测后不重新做全局 BA而是只优化关键帧位姿固定地图点用本质图上的边作为约束。为什么不用共视图做位姿图优化因为共视图边太多位姿图优化虽然比 BA 快但边太多仍然吃不消。本质图把边数压到接近扩展树的量级同时保留了回环这种关键约束所以能在秒级完成全局位姿修正。回环后的地图复用、重定位都依赖本质图上的位姿一致性。2.6 四者关系一句话总结关键帧是节点共视图描述节点间的观测重叠用于局部 BA扩展树是共视图的骨架用于全局初始化和回环候选本质图是共视图的精简加回环边用于位姿图优化。四者层层递进共视图最稠密扩展树最稀疏本质图居中但包含回环。3. 从代码视角拆解四张图的构建与更新3.1 关键帧的创建与地图点关联在 ORB-SLAM2 里关键帧由 Tracking 线程决定是否插入然后交给 LocalMapping 线程处理。创建关键帧时会做几件事计算位姿、提取 ORB 特征、创建地图点、更新共视图和扩展树。关键帧和地图点之间是多对多关系一个关键帧能看到多个地图点一个地图点也能被多个关键帧看到。这里有个细节地图点的观测列表里存的是关键帧指针和该点在该帧中的特征索引。这个观测列表是共视图权重计算的依据。如果你在做地图点融合或剔除必须同步更新观测列表否则共视图权重会算错。3.2 共视图的更新时机与权重计算共视图的更新主要发生在两个地方一是新关键帧插入时计算它与已有共视邻居的边二是地图点融合或剔除后重新计算受影响关键帧之间的共同观测数。ORB-SLAM2 里用UpdateCovisibilityGraph之类的函数来维护。权重计算很简单两个关键帧共同观测到的地图点数量。但要注意这里的地图点必须是“有效”的也就是没有被标记为 outlier 的点。如果你在外部改了地图点状态一定要触发共视图更新。实操心得调试共视图时可以打印某个关键帧的共视邻居数量和权重分布。如果发现权重普遍很低可能是地图点质量差或者特征匹配阈值太松。3.3 扩展树的生成与父子关系维护扩展树是从共视图里用类似最小生成树的算法抽出来的但 ORB-SLAM2 的实现更简单每个新关键帧插入时选择共视权重最高的邻居作为父节点自己作为子节点加入。如果后续共视图变化导致原来的父节点不再是最优ORB-SLAM2 并不会主动重连扩展树除非发生关键帧剔除。关键帧剔除时扩展树需要把被剔除节点的子节点重新挂到它的父节点上并更新共视关系。这个逻辑在LocalMapping::DeleteKeyFrame附近改代码时容易漏掉。3.4 本质图的边选择策略本质图的构建在回环检测之后。ORB-SLAM2 会遍历所有关键帧把扩展树的边、共视权重超过阈值的边、以及回环边加入本质图。阈值通常设得比共视图边阈值高比如共视图是 15本质图可能是 100。这样本质图的边数远少于共视图但保留了强约束和回环约束。位姿图优化时本质图上的边作为约束用 g2o 或类似库求解。优化变量是关键帧位姿地图点固定。优化完成后地图点和关键帧的位姿会更新但共视图和扩展树通常不重建除非触发全局 BA。3.5 四张图在三个线程中的流转Tracking 线程负责跟踪决定是否插入关键帧LocalMapping 线程负责局部 BA、共视图和扩展树更新、关键帧剔除LoopClosing 线程负责回环检测、本质图构建和位姿图优化。三个线程通过关键帧队列和地图点交互。理解这个流转才能知道在哪个环节改代码会影响哪张图。4. 实操从零复现四张图的构建与查询4.1 环境准备与代码入口定位假设你已经能编译运行 ORB-SLAM2。先找到几个关键文件KeyFrame.h/cpp、Map.h/cpp、LocalMapping.h/cpp、LoopClosing.h/cpp。关键帧的共视关系存在KeyFrame::mConnectedKeyFrameWeights里扩展树的父子关系存在KeyFrame::mpParent和mspChildren里本质图在LoopClosing::ComputeEssentialGraph附近构建。4.2 打印某关键帧的共视邻居与权重你可以在KeyFrame::UpdateConnections之后加一段打印输出当前关键帧的 ID、共视邻居数量和权重最高的前五个邻居。跑一段数据集观察权重分布。如果发现某些关键帧的共视邻居数为零说明它可能是孤立关键帧需要检查地图点关联。// 示例打印共视邻居 std::vectorKeyFrame* vNeighbors pKF-GetVectorCovisibleKeyFrames(); std::cout KF pKF-mnId has vNeighbors.size() covisible neighbors std::endl; for (auto* pNeighbor : vNeighbors) { int weight pKF-GetWeight(pNeighbor); std::cout neighbor pNeighbor-mnId weight weight std::endl; }4.3 可视化扩展树与本质图ORB-SLAM2 本身不带图可视化但你可以把扩展树的父子关系导出成 DOT 格式用 Graphviz 画出来。本质图同理导出边列表后画图。这样能直观看到扩展树是否退化成链、本质图是否包含回环边。注意导出时只导出关键帧 ID 和边不要导出真实场景信息避免隐私问题。4.4 模拟回环并观察本质图变化用数据集跑一段有回环的序列在回环检测触发前后分别导出本质图。你会看到回环边被加入本质图位姿图优化后关键帧位姿发生跳变。对比优化前后的轨迹能直观理解本质图的作用。4.5 参数对照表图结构边阈值/条件主要用途更新时机共视图共同观测 ≥ 15局部 BA新关键帧、地图点融合扩展树共视权重最高全局初始化、回环候选新关键帧、关键帧剔除本质图扩展树边 高权重边 回环边位姿图优化回环检测后关键帧插入条件满足节点基础Tracking 决定5. 常见问题与排查技巧实录5.1 回环后地图歪掉本质图边太少或太多如果本质图边太少位姿图优化约束不足回环修正无法传播到全局边太多优化慢且可能过约束。排查时先打印本质图边数对比关键帧数量。正常情况边数应是关键帧数的 1.5 到 3 倍。如果接近 1说明扩展树退化成链如果超过 5说明阈值太低。5.2 局部 BA 后轨迹抖动共视图权重异常局部 BA 依赖共视图权重。如果权重计算错误比如把 outlier 也算进去优化会引入错误约束。排查时检查地图点观测列表确认 outlier 被剔除。另外共视图更新不及时也会导致权重滞后。5.3 重定位失败本质图位姿不一致重定位依赖本质图上的位姿一致性。如果位姿图优化没有正确更新所有关键帧或者本质图没有包含回环边重定位会失败。排查时对比优化前后关键帧位姿确认回环边存在。5.4 关键帧剔除后扩展树断裂关键帧剔除时如果子节点没有正确重连扩展树会断裂导致全局 BA 或回环候选搜索失败。排查时检查被剔除节点的子节点是否挂到了父节点上。5.5 常见问题速查表现象可能原因排查方法回环后地图歪本质图边不足打印边数检查回环边局部轨迹抖动共视图权重错误检查地图点 outlier重定位失败本质图位姿不一致对比优化前后位姿扩展树断裂关键帧剔除未重连检查父子关系优化慢本质图边太多提高阈值5.6 独家避坑技巧改共视图阈值时不要只改一个地方ORB-SLAM2 里多处硬编码了 15 和 100要全局搜索替换。另外调试时可以先关掉回环单独观察共视图和扩展树再开回环看本质图分阶段排查。6. 我个人在实际操作中的体会这套图结构刚看的时候觉得绕但真正跑过几遍、打印过几次边和权重之后就会发现它其实很符合工程直觉局部问题局部解决全局问题用骨架加关键约束解决。我后来做多传感器融合时就是把别的约束作为额外边加进本质图而不是去动共视图这样对原有系统影响最小。如果你也在改后端建议先从打印和可视化入手把四张图的实际形态看清楚再动代码。