Month: March 2025

这篇文章将向大家总体介绍一下ORB-SLAM3系统的设计和实现，其中每个模块的实现逻辑和思路已经在主题文章中向大家做了介绍。 ORB-SLAM3实现了多种传感器配置下的同步定位（相机位姿的实时跟踪定位）和建图（基于稀疏三维地图点的三维地图构建）功能，且通过Atlas类实现了多地图集（跟踪失效时会重新创新新地图）的管理，以及包括地图的回环检测和矫正以及地图的合并。整个系统大体由单帧实时跟踪模块（Tracking.cc），局部建图模块(LocalMapping.cc)和回环检测和矫正（LoopClosing.cc）模块以及可视化模块（View.cc）等构成。其中各种传感器配置场景在代码实现时是耦合在一起的，根据配置的条件的不同对其逻辑的差异性分别按条件进行处理。 Tracking模块需要处理频率较高的视频帧，采用参考关键帧或基于上一帧的运动跟踪模式对特征点进行匹配和相机位姿进行优化等。一般如果处理一帧的tracking较为耗时，可能要降低帧率或丢弃部分帧，一般来说，tracking一帧的耗时可以在帧率的一半，而仅有部分帧将作为关键帧来加入到地图中，关键帧帧率大概在0.5-2秒左右，因此关键帧占总帧数的比率较低，即使在tracking的全过程中使用地图更新访问加锁，其他模块如局部建图和回环检测和矫正模块都会有较为充裕的时间来调度执行。 这里对于模块涉及地图更新的操作的地方做一下说明。首先在Tracking过程中涉及地图更新的操作的地方有：（1）单目初始化函数MonocularInitialization里调用了初始地图的创建，将当前帧和初始帧作为两个关键帧插入了初始地图里；（2）TrackReferenceKeyFrame或TrackWithMotionModel函数里对相机位姿进行了优化（也更新了地图点的信息）；（3）重定位时从候选的关键帧中计算匹配点并优化位姿；（4）、TrackLocalMap利用了关键帧邻近关键帧的更多的匹配地图点进行位姿优化；（5）最后判断是否将当前帧作为关键帧插入，如果插入关键帧，同样也需要更新地图。 问题：ORBSLAM3中的LocalMapping的函数SetAcceptKeyFrames(false);只限制了Tracking模块不能插入新的关键帧，但如果在LocalMapping的过程中如实现冗余关键帧剔除（KeyFrameCulling）或者地图点的剔除操作的同时，有没有可能和Tracking的访问地图操作存在着数据访问竞争的问题，因为在LocalMapping的处理过程中好像没有加锁访问地图更新？ 回答：在局部建图的处理过程中虽然没有使用当前地图的互斥量mMutexMapUpdate进行加锁，但是在地图的细粒度的访问中都加了细粒度的互斥量的定义，同时ORB-SLAM3不追求严格的实时一致性，而是通过以下设计实现最终一致性：（1）闭环校正的全局同步：当检测到闭环时，系统通过全局BA或位姿图优化对所有关键帧和地图点进行全局调整，此时所有线程暂停，确保全局状态的一致性。（2）高频跟踪，低频优化：如Tracking线程以相机帧率（30Hz）运行，而LocalMapping和LoopClosing线程以更低频率（约10Hz）执行优化。优化结果对Tracking的影响是“延迟生效”的，但系统通过BA和闭环校正逐步收敛到全局一致状态，如在Tracking的高频计算过程中，低频的局部建图和回环检测运行的数据是基于时间线上靠前面的数据，从而也减少了冲突访问的可能性。（3）、采用一些鲁棒的核函数和优化算法，即使存在着临时的量的不一致性，也能足够健壮得出较为理想的优化结果。（4）、局部BA等优化过程更新相机位姿和地图点时采用了地图更新互斥量这种粒度较大的锁，实现了和tracking过程的互斥访问。 关于ORB-SLAM3系统中可能存在的bug问题，可以参考文献[7]中作者给出的修正。ORB-SLAM3系统主要的功能是定位和建图，在机器人无人机等系统中作为感知的大模块的一部分，也需要和PNC（Planning and Control）等模块集成形成一个完整的自主导航系统。 References

本篇文章将向大家介绍在SLAM中使用的相关优化算法及其在ORB-SLAM3中相关模块中的具体应用案例。 首先，SLAM系统的核心功能之一为视觉里程计（Visual Odometry, VO，也通常称为前端，记录智能体随时间变化在场景中运动的姿势方向和位移，由于前端长时间运行会出现累积漂移和误差，因此也还有回环和地图合并检测等后端模块来进行误差矫正和消除）. 在系统初始化或tracking跟踪丢失创新新地图时，地图初始化时的第一帧的位姿设定为单位矩阵，第二帧的位姿以及两个连续帧的匹配特征点对应的地图点初始值通过Camera类的ReconstructWithTwoViews函数进行计算实现，具体是通过对极约束和三角测量来分别估计相机运动位姿和求解像素的深度信息和对应的物理世界的地图点坐标）。 MLPnPSolver优化器，该优化器的作用为基于最大似然的PnP（Perspective-n-Point）问题求解器，PnP问题的一种典型应用场景为基于匹配的前后帧的3D坐标和2D图像特征点集合来估计相机运动的位姿（旋转+平移），在Tracking的重定位算法中用于快速找出相机位姿的初始解供后续进一步优化。MLPnPSolver基于最小二乘的高斯牛顿法迭代求解逐步减小重投影误差来优化相机位姿，其中MLE(最大似然估计）是一种基于统计概率的参数估计方法，在SLAM应用场景中，参数为相机位姿，观察值为图像中ORB特征点的像素坐标，通过MLE算法，找到最优的参数解等价于使得重投影误差最小化。在算法的实现过程中，由于图像的位姿如旋转矩阵的参数对于误差函数不方便直接求导，而是通过罗德里格斯公式将旋转矩阵转化为旋转向量，和位移一起构成6维向量根据李代数的扰动模型进行求导，而在进行优化时，误差项e为图像uv平面的二维向量。因此目标函数将表现为向量的形式，基于雅可比的矩阵导数的优化比较直观，在大规模问题场景中（如基于地图的全局位姿和地图点优化），利用雅可比矩阵的稀疏性能更高效求解。在MLPnPSolver中，同时采用了RANSAC（Random Sample Consensus，重采样一致性算法）算法，RANSAC是一种鲁棒估计方法，其也采用了多次迭代求解，每次迭代随机选择一定数量的匹配3D2D点对进行求解，并统计内点（求得的位姿进行匹配点映射后误差在一定小的范围内）的数量，最后选择内点数量最多的为最后的位姿优化结果。 Optimizer类的相关优化函数；（1）、PoseOptimization，基于3D2D匹配点对和已经根据Tracking的过程优化的初始位姿，采用g2o的图优化方法来优化位姿，其中g2o的图优化中，节点有基于位姿的g2o::VertexSE3Expmap，边有ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose（顾名思义，这条边仅仅通过相机位姿SE3将地图点坐标映射到图像像素且仅仅优化位姿Pose），以及设置观测值（像素坐标），核函数等，（2）、LocalBundleAdjustment，基于传入的关键帧及其共视关键帧集合对多帧位姿和地图点进行联合优化，同样基于g2o的图优化算法，这里有一个需要注意的地方在于地图点的观测帧不属于共视关键帧的观测帧将作为位姿固定的节点（不参与位姿优化的节点）加入到优化的graph里。这里用的边为EdgeSE3ProjectXYZ，将同时优化位姿和地图点；（3）、GlobalBundleAdjustemnt和BundleAdjustment将以关键帧数组和地图点数组来构建g2o优化图进行联合位姿和地图点优化。(4)，Optimizer还有一些和IMU相关的联合优化方法实现，具体可以参考文献2中的代码实现[2]。 Sim3Solver：Horn’s 四元数方法（Horn’s Method）是一种用于最优对齐两个三维点集的方法，广泛用于位姿估计、点云配准、结构光扫描等场景。它通过最小化均方误差（Least-Squares Error），利用四元数来高效求解旋转矩阵R，在ORB-SLAM3系统中，其主要用于（1）、回环匹配关键帧和当前关键帧的变换求解；求解的参数有旋转、平移和尺度信息，为七个自由度；（2）、合并匹配关键帧和当前关键帧的变换求解。 References

这篇文章将向大家介绍LoopClosing模块中的地图回环矫正和地图合并和矫正部分的内容，其函数分别为CorrectLoop和MergeLocal（或MergeLocal2），其根据从回环检测过程中求解出的回环匹配关键帧或合并匹配关键帧到当前帧的Sim(3)变换进行回环和地图合并及矫正。 关于回环矫正的几个问题： 问题1： 在ORB-SLAM3中CorrectLoop的函数[1]的功能中根据前面优化求得的变换mg2oLoopScw应用到当前关键帧及其共视帧（统称为连接帧mvpCurrentConnectedKFs）上，然后进行OptimizeEssentialGraph进行优化，请详细分析一下具体的步骤及思路。 回答：mg2oLoopScw是 Sim3Solver计算得到的回环帧到当前帧的 Sim(3) 变换所得矫正后位姿，用于修正关键帧及其连接帧集合（当前关键帧的所有共视关键帧）的位姿（将该位姿和调整关键帧和当前关键帧的相对变换进行叠加及可以对每一个共视关键帧的位姿进行初步矫正），使整个局部关键帧集合做统一一致的初步矫正。然后调用OptimizeEssentialGraph进行通常在闭环检测后调整关键帧的位姿，保持整体一致性。这里使用sim(3)变换，因为闭环可能涉及尺度漂移，需要调整尺度。(而GlobalBundleAdjustment是全局BA，优化所有关键帧和地图点的位置，使用se(3)因为BA通常处理的是刚体变换，不考虑尺度，将回环约束均匀传播到整个关键帧图)。关于优化部分的内容，将会在专门的文章中加以介绍。关于回环矫正的更详细的思路，可以参考本文后面地图合并的部分内容。 问题2：首先将当前关键帧及其共视关键帧应用一致的矫正变换然后再进行全局优化。这个步骤不添加是否会有影响？ 回答：一般的情况是回环匹配帧由于在地图的时间点前面，其误差和尺度漂移的程度要小，当前关键帧和回环匹配帧之间也许走过不少的其他关键帧，积累的较多的误差，因此首先通过将当前关键帧的共视关键帧首先应用统一的矫正变换以实现初步的累积误差和尺度漂移矫正，这对于后面的全局优化的过程能够更加容易收敛。 关于地图合并的几个问题： 问题1：地图合并的场景有没有比较具体的应用场景的示例帮助理解？ 回答：ORB-SLAM3支持多地图(multi-map，由Atlas类来管理地图集)，在地图合并和回环状态的检测的过程中不同的地方在于匹配关键帧和当前关键帧在不在同一个地图里。比如家庭里的多个房间，工厂里的多个车间等。 问题2：地图合并时的实现逻辑是怎样的？ 回答: 地图合并时主要采取如下的步骤：（1）、首先根据当前帧来更新一定窗口大小的共视帧数量（如果当前关键帧的直接共视关键帧的数量不够，则继续在共视帧的共视帧中去扩展），同理也对合并匹配帧去获取其共视帧的集合。（2）、然后对当前关键帧窗口内的关键帧采用和回环纠正第一步的Sim(3)变换一样的操作实现初步的位姿矫正以便于后续的全局优化的迭代收敛。（3）、执行地图合并操作，将当前帧相关的关键帧（spLocalWindowKFs变量里的当前这的局部窗口内的帧的集合，包括前后一段时间内的连续关键帧和共视关键帧等）合并到融合的地图里pMergeMap->AddKeyFrame(pKFi)，并从当前的地图里移除对应的关键帧pCurrentMap->EraseKeyFrame(pKFi)。地图点的合并过程类似。然后Atlas地图集管理将融合地图设为当前地图（表示当前系统已经实现了地图跳转），并将当前地图标记为失效地图；（4）、将当前关键帧的父关键帧设为合并匹配关键帧，并逆序当前关键帧的父子关系链条；（5）、在合并后有些地图点存在着重复（比如合并匹配帧及附近的共视关键帧和当前关键帧及附近的共视关键帧有些地图点是重复的），通过sim(3)变换后去搜索这些重复的地图点并剔除冗余（调用SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint)函数），针对每一个当前关键帧邻近窗口内的关键帧集合和融合关键帧相连接的关键帧集合的每一个关键帧调用UpdateConnections更新相关连接信息；（6）调用函数Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop)进行局部优化，其中vpMergeConnectedKFs参数中的关键帧的位姿为fixed不参与优化（与优化的关键帧共享的地图点会参与优化，但也会受约束于固定的位姿，这给优化的过程提供了基准参考，防止优化出现漂移现象）。（7）对当前地图的没有融合到pMergeMap部分的关键帧同样采用Sim(3)矫正变换以保证整个地图的一致性，并再一次通过加锁机制实现地图的合并操作（相关关键帧和地图点在地图集里的关系更新，在代码的实现上和第三步的合并操作一样）（8）调用全局线束优化，对合并后的当前地图的所有关键帧位姿和地图点集合进行联合优化，以消除较长时间运行的累积误差，在优化前会让局部建图器停止工作。 问题3：地图合并的时候，将当前地图设置为失效，下次合并的时候也可能会再次变为当前地图对吗？ 回答：是的，在 ORB-SLAM3 中，地图合并时将当前地图设置为“失效”（即 SetMapBad()），目的是标记当前地图已经不再使用，并且可能在之后的合并过程中被再次激活作为当前地图。 问题4：由于在地图合并的时候存在着关键帧父子关系链的调整，会出现两个子问题：（1）、将当前关键帧的父关键这设为融合匹配关键帧，并调整当前关键帧的父子关键帧链条上的顺序结构，则之前的融合匹配关键帧的子关键帧链条如何维护？(2)、关键帧之间的父子关系链和关键帧之间的前后顺序双向链表定义是不同的数据结构意义，双向链表主要体现在时间上的先后顺序，父子关系主要体现在共视的重合程度吗？ 回答：（1）、在支持多地图的合并场景下，地图的合并将会形成在合并点出现树的分叉结构（每次融合可以理解将融合关键帧的现有子关键帧链保持，同时添加一个当前关键帧的子关键帧链条），这种分叉结构可以保证父子关键帧关系图不会出现闭环形结构。（2）、父子关键帧在一般情况下保持时序（一般在mbFirstConnection为true时建立父子关系），且主要体现了关键帧之间的共视重合度，但在上述的合并地图的场景下，为了优化地图结构，会调整关键帧之间的父子关系结构，此时部分父子关系链上的结构将不再保证时序关系。关键帧里的双向链表结构确实保持了关键帧的时序关系。可以方便获取当前帧在时间序列上的前一个和后一个关键帧。 问题5：地图合并时由于每个地图的坐标系独立，即一般在跟踪失效的时候会创建新地图，新地图的初始关键帧的坐标为标准的单位矩阵？在两个地图的融合时坐标如何对齐？和回环检测和纠正时的处理有什么具体的差异？ 回答：一般在回环检测和矫正以及地图合并检测和合并的处理过程中，会以先前的回环匹配帧或合并匹配帧作为基准（后面经过了一段时间的累积出现了累积的漂移误差），基于两个匹配帧的变换将当前帧及当前地图的所有帧都首先进行Sim(3)的变换进行初步纠正，这个变换逻辑是一样的。 References