学问思辨行: 记录生活学习和工作中的实践和思考，以期实现终身成长.

这篇文章将向大家总体介绍一下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

这篇文章将向大家介绍回环和地图合并检测相关实现逻辑，和局部建图器类似，回环检测实现的类为LoopClosing（该类同时也实现了回环矫正和地图合并及矫正，其中矫正和合并的内容将于后续文章介绍），和局部建图器模块一样，该模块也在背景线程中运行，线程运行函数为LoopClosing::run()，为一个随着系统一直运行的while(1)循环。该模块的总体功能是检测地图回环和可能的地图合并，整体地图的维护和所有关键帧的位姿和地图点的全局优化，这篇文章将向大家介绍LoopClosing中的地图回环及合并检测。具体的回环和合并矫正的内容将在下一篇文章中较为详细的分析介绍。 回环检测首先从回环检测队列中取出一个关键帧进行处理，首先调用NewDetectCommonRegions函数来判断当前帧和历史帧是否存在着共同的区域，如果存在，有可能是检测到了回环或者是合并的场景，如果合并条件成立，则调用MergeLocal或MergeLocal2对地图进行合并；如果检测到回环，则进行回环校正。 NewDetectCommonRegions函数的大体逻辑是如果还没有检测到回环或合并发生的次数（mnLoopNumCoincidences，mnMergeNumCoincidences刚开始回环检测或者期间没有检测到匹配候选时重置为0）。回环的检测并不是在当前帧与回环候选关键帧之间的第一次匹配时就直接确认回环存在，而是需要连续几帧都与回环候选帧的匹配度和几何验证通过，才会最终确认回环。其中mnLoopNumCoincidences为检测到回环发生的次数记录，mnLoopNumCoincidences>3说明需要至少连续三帧的匹配才算有效的回环检测，以增加回环检测的鲁棒性，合并状态检测的情形类似。 NewDetectCommonRegions函数首先通过调用KeyFrameDatabase类的DetectNBestCandidates函数根据当前帧和关键帧数据库中关键帧的特征点对应的属性“单词”word形成的“文档”（BOW，bag of words）采用余弦相似度或者TF/IDF为度量标准进行相似性计算找出回环关键帧候选集合和合并关键帧候选集合（在同一个地图的为回环候选关键帧，在不同的地图中的为合并候选关键帧），其中BOW为一个“文档向量”，向量中的每一个元素可以表示特征对应的属性单词是否在该关键帧的在词典中出现，关于BOW的相关实现，可以参考文献[2]中的ORBVocabulary的介绍。 然后两次调用DetectCommonRegionsFromBoW函数分别从候选匹配帧集合中找出是否存在回环或需要合并的匹配关键帧，其基本思想是遍历上一个函数返回的候选的回环候选帧和合并候选帧的集合，将每一个候选和当前关键帧进行匹配相似度计算，并采用Sim3Solver进行几何验证，返回最佳的匹配候选帧，该函数分别调用两次，将最优的匹配结果分别更新到变量mpLoopMatchedKF和mpMergeMatchedKF中去。同时通过计算分别获得mg2oLoopSlw和mg2oMergeSlw变量，这两个变量的求解过程使用到了Sim3Solver算法[1]，该变量的含义为从回环匹配关键帧或合并匹配关键帧到当前关键帧进行Sim(3）变换进行纠正后的位姿。 如果检测到了回环的存在，则调用CorrectLoop函数进行回环矫正，如果检测到了合并状态的存在，则调用MergeLocal或MergeLocal2（有IMU的场景）进行地图的合并并进行位姿和地图点的矫正。具体的回环矫正和地图的合并的方法将在后续专门的博客文章里向大家介绍。 References

本篇文章将向大家介绍局部建图器的代码逻辑，局部建图器的类名为LocalMapping，在独立的背景线程中运行，和主线程的跟踪逻辑之间可以并发执行。局部建图器模块的主要作用是当前处理的关键帧及其邻近关键帧的位姿和地图点的联合优化。 LocalMapping的run函数为线程启动的函数，该函数在启动后一直循环运行，按顺序从现有的尚未处理的关键帧列表中依照时间先后顺序pop出关键帧进行处理(调用函数为ProcessNewKeyFrame)，ProcessNewKeyFrame的处理逻辑大体为：1、从关键帧列表中按时间顺序pop出一帧关键帧进行处理；2、在先前已经进行ORB特征点和特征描述符的基础上计算BOW以用于后面的回环检测，检查和更新地图点的观测关系；3、通过调用UpdateConnections函数更新关键帧的共视图相关属性，如连接的共视帧及其共享地图点等信息，具体解释可以参考文献[1]中的详细介绍。以及在地图中插入关键帧。 MapPointCulling函数对最近加入的地图点（如通过立体视觉加入，或者最近在跟踪的过程中发现的新的地图点）执行地图点清除工作，将发现率较低的点，较长时间未被观测到的等一些质量较低的地图点清除，以保证地图点的质量。 CreateNewMapPoints的功能是：1、通过关键帧找到最佳的邻近的共视关键帧集合；2、当前关键帧和每一个邻近的共视关键帧中的关键点之间进行特征点匹配，并将匹配的但尚未分配地图点的特征点进行三角化测量算法求出新的地图点，并更新地图点和关键帧之间的关联关系等，最后将地图点加入到地图点集合变量mlpRecentAddedMapPoints中。 调用Optimizer::LocalBundleAdjustment进行局部线束调整优化，在优化时，以当前的关键帧及其共视的关键帧作为优化的范围，更新关键帧集合和地图点集合，并采用g2o的基于李代数的Levenberg-Marquardt 算法来联合优化多帧相机位姿和地图点坐标。 通过调用KeyFrameCulling进行冗余关键帧剔除操作，其粗略主线逻辑为：首先根据当前关键帧获取最佳共视邻近关键帧集合；然后对于集合里的每一个关键帧，统计其地图点的观测属性，如果发现超过一定量的地图点都被较多的重复观测到，则定义该关键帧为冗余关键帧，标记该关键帧为无效（SetBadFlag），减少地图中的关键帧数量，简化后续的优化过程。 接下来的步骤中需要判断是否有IMU支持，如果有IMU的场景下，则在系统启动时间（minit的变量）一定范围内对IMU启动两轮惯性标定优化，然后根据设定的时间窗口综合IMU的测量和关键帧数据（关键帧和地图点数据）对地图的尺度进行调整，确保地图的精度。 最后通过调用mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame)语句，将当前关键帧加入到闭环检测模块中。关于回环和合并检测，可以参考后续相关连载主题详细说明。 References

这篇文章将向大家介绍orbslam3开源系统中的单目（+惯导）跟踪算法模块的实现逻辑分析。 跟踪模块在orbslam3中作为计算频率最高的算法模块，在实现中也作为程序的主线程来运行，其他的模块如局部建图（LocalMapping），回环检测（LoopClosing）以及可视化模块（Viewer）均以其他背景线程来并发运行。跟踪模块的主要作用为相机的运动模型（位姿）的实时跟踪和优化。 跟踪模块作为整个系统的入口，其接收从感知传感器捕获的环境数据，如图像数据，IMU，Lidar等，这里将以单目(+imu)场景，且以跟踪+定位（也有只跟踪的模式）的模式为例来进行分析介绍。 一、接收图像并进行处理的函数调用路径（以mono_euroc的example为例）为mono_euroc.cc文件中的SLAM.TrackMonocular(im,tframe)->system.cc文件中的mpTracker->GrabImageMonocular(imToFeed,timestamp,filename)->tracking.cc文件中的GrabImageMonocular函数将图像先转换成灰度图，然后构建Frame类的对象，最后调用Track()函数进行跟踪。这里对Frame构造数据帧对象做一下补充说明：Frame类支持多态的构造函数以支持多个应用场景，如单目，双目，深度相机等，在构造函数里首先直接在畸变的图像上进行ORB特征点和特征描述符的计算提取，然后对特征点进行去畸变的处理。 二、跟踪过程初始化：跟踪过程先判断当前的跟踪状态，跟踪的状态候选有[NOT_INITIALIZED, LOST, RECENTLY_LOST, OK]，当判断到系统当前跟踪状态还尚未初始化时（状态为NOT_INITIALIZED），则首先进行单目的初始化工作，其实现函数为MonocularInitialization，初始化的状态由mbReadyToInitializate变量来表示，系统启动时为false，其状态的判别和转换的逻辑详见[1]，完整的初始化过程需要两个间隔较短的图像帧且两帧的特征点的数量都超过了设定的阈值。当满足这样的条件时，根据这前后临近的两帧的匹配特征点集合基于对极几何和三角化测量方法（ReconstructWithTwoViews）去初始化相机位姿和地图点坐标，如果成功初始化相机位姿，则调用函数CreateInitialMapMonocular创建初始地图，将初始帧和当前帧作为关键帧加入到当前地图里，同时更新对应的地图点和关键帧之间的关联关系，并进行全局的BA优化以进一步优化相机初始位姿。 三、当跟踪状态为OK时，如果运动模型不可用，则调用TrackReferenceKeyFrame，如果运动模型可用，则先调用TrackWithMotionModel，如果用运动模型跟踪不成功，则回退到用TrackReferenceKeyFrame进行跟踪。其中: (1)、TrackReferenceKeyFrame将当前帧和参考关键帧（临近创建的关键帧）进行特征点匹配（如果匹配的特征点不够，则返回跟踪失败，需要进行重定位），并基于此通过Optimizer的PoseOptimization函数对当前帧进行位姿优化；（2）、TrackWithMotionModel根据运动模型预测当前的相机位姿，并将当前帧和上一帧进行投影搜索，如果匹配点过少，将加大阈值再次将当前帧和上一帧进行投影搜索，满足匹配条件则跟踪成功调用PoseOptimization函数进行优化，否则如果有IMU辅助预测，直接返回true，如果既搜索失败又没有IMU辅助预测，则返回false。TrackReferenceKeyFrame和TrackWithMotionModel分别基于参考关键帧和运动模型对当前帧的特征点进行匹配，并得到和参考关键帧或上一帧所匹配的特征点对应的地图点，Optimizer的PoseOptimization函数主要采用了前面帧得出的地图点世界坐标和对应的当前帧关键点的运动映射所建立的最小化重投影误差模型对相机的位姿和地图点进行联合优化，其具体采用了李代数和g2o的图优化方法，具体的介绍见后续的相关博文。 四、当跟踪的状态为RECENTLY_LOST，如果有IMU，则用IMU进行状态的辅助预测，保持状态为RECENTLY_LOST不变；否则调用Relocalization函数进行重定位，如果重定位成功，则状态转换为OK，否则状态转换为LOST。这里对重定位函数的实现也做一下说明，其主要基于当前帧从地图里查找候选关键帧(调用mpKeyFrameDB->DetectRelocalizationCandidates去检测)，然后基于每个候选帧分别和当前帧进行位姿优化（先采用的MLPnPSolver的优化求解方法，实时速度快，只优化相机位姿，找到匹配较优秀的结果作为初始值，和PoseOptimization相结合去进行进一步的更高精确度的位姿优化，具体的实现逻辑参考[2]。 五、当跟踪的状态为LOST，则重新创建新的地图，开始进入新的跟踪初始化状态。 六、当前帧本身的跟踪任务完成后，接下来需要调用TrackLocalMap对局部地图进行跟踪和相关的位姿和地图点的优化，在上一篇文章里也介绍过，局部地图没有专门的封装类，其主要的信息数据在Tracking类的两个变量中，分别为mvpLocalKeyFrames和mvpLocalMapPoints，在实现内部，其通过UpdateLocalMap函数的调用实现了局部关键帧和局部地图点的更新（只对当前的参考帧的匹配的地图点作为约束，约束量偏少，在更大的局部地图点中去搜索更多的匹配点，从而有更多的地图点加入到重投影误差的计算中来，对当前帧的位姿优化也更加准确），然后通过函数SearchLocalPoints去搜索匹配和当前帧在视锥体范围内的匹配的特征点，并基于这些特征点和地图点以及相机位姿等信息进行相机位姿优化。 七、接下来需要通过函数NeedNewKeyFrame来判断是否需要加入关键帧，根据多种条件，比如时间，比如有足够多的新的“接近点”等等，如果需要创建关键帧则调用CreateNewKeyFrame函数来创建关键帧，并将关键帧插入到局部建图器以便进行局部建图的工作。其中局部建图器的大体作用有：1、从尚未处理的关键帧队列里取出关键帧，对关键帧的地图点相关信息进行更新；2、更新关键帧的共视关键帧的图连接；3、将关键帧加入到地图集的当前地图中。这些具体的内容将会在局部建图器的文章中专门介绍。 问题：TrackLocalMap也是通过地图点和特征点的重投影误差采用的Optimizer的PoseOptimization函数进行的相机位姿和地图点的联合优化，为什么在当前帧的跟踪后面还要加上这个过程，其利用了更多的关键帧搜索地图点，因此优化会更准确些吗?回答：在 跟踪阶段，位姿优化依赖当前帧与前一帧（或参考关键帧）之间的特征点匹配关系，但 TrackLocalMap 可以通过当前帧的地图点去搜索局部的多个共视关键帧，查找更多的匹配的地图点。以从更多的视角来优化进一步提升定位精度和地图精度，使得SLAM系统在面对动态场景或挑战性环境时更加稳健。 References

感谢读者的关注，近期准备向大家介绍ORB-SLAM3的源码解读，准备主要从基本的数据结构、跟踪模块、局部建图模块、回环检测和回环纠正模块、可视化模块以及系统概览这些主题内容分别向大家介绍。欢迎大家的反馈和意见建议以便纠错和补充更新。 这篇短文将向大家介绍ORBSLAM3的基本关键的数据及相关计算的抽象，具体表现为以下几个有代表性的类， 帧（Frame），图像帧数据结构，是视觉slam中基本的传感器单元数据，在单目场景下，一帧的数据为camera某个时间戳对应的图像数据。帧的类中还包含对图像进行ORBExtractor所获取的特征点，特征描述符以及BOW（bag of words，将特征描述符向量根据k叉树结构的词汇表模型进行遍历搜索获得，便于后面的回环检测算法的需要）。帧的类实现中还包括保存优化后当前帧对应的相机的位姿(mTcw)，在优化的过程中还有关键点对应的地图点（成员mvpMapPoints，见MapPoint部分的介绍），帧中也维护了camera的内参信息，以及如果是双目立体视觉场景，一帧的数据中还包含右侧camera的图像数据，以及对应的特征点等相关数据。 关键帧（KeyFrame），并不是所有的帧都作为关键帧（这里借鉴了视频压缩里类似的概念，由于camera的帧率一般较高，进行局部和全局相机位姿和地图点优化时一般以关键帧的数据为优化对象），在跟踪逻辑里，会检查是否需要将当前帧作为关键帧的逻辑，比如根据时间和跟踪效果等（具体在跟踪逻辑在NeedNewKeyFrame函数里进行的实现[1]）。关键帧主要用于局部地图位姿优化和全局位姿优化，也是构成地图数据的基本单元。keyframe的类实现中也对共视关键帧进行了维护，通过地图点结构就可以得出共视的关键帧，具体的函数实现为UpdateConnections函数，其根据和当前关键帧共视地图点的数量对共视的关键帧进行排序，其中变量mvpOrderedConnectedKeyFrames按和当前关键帧共视的地图点数量的多少将共视关键帧进行排序存放到数组里，mvOrderedWeights存放对应的共视的地图点的权重（数量），mConnectedKeyFrameWeights记录共视关键帧和共视地图点数量的映射关系（以STL中的字典map容器实现）。并根据需要建立关键帧之间的父子关系（父子关键帧通常共享大量的地图点，如果当前关键帧为第一次建立连接且不是地图的初始关键帧则选择排序中的第一个共视关键帧为父关键帧），关键帧按照先后顺序构建成双向的链表，CreateNewKeyFrame的函数里有对这个双向链表进行维护的操作。 关键帧数据库（KeyFrameDatabase），存放了关键帧的集合，并且实现了基于BOW等信息的关键帧查找的一系列方法函数供相关模块使用，比如在回环检测模块，提供了基于根据当前帧查找相似度较大的回环匹配帧候选集合和合并关键帧候选集合等。 地图点（MapPoint），地图点为物理世界中的具体位置点的抽象，包括地图点的世界坐标，观测到该地图点的关键帧集合（mObservations），以及对应的每个关键帧图像中的哪个特征点和该地图点相关联等信息（双目的时候分左右camera的特征点的关联索引）。 地图（Map），地图管理的类里包含了关键帧集合(mspKeyFrames)和地图点的集合(mspMapPoints)[2]，在orbslam3中没有专门的类用来抽象局部地图，局部地图的相关数据的维护和更新体现在tracking模块（代码见tracking.cc)的两个相关的成员变量：局部关键帧（mvpLocalKeyFrames）和局部地图点（mvpLocalMapPoints）中，通过TrackLocalMap()函数根据当前帧进行更新和维护[3]。 地图集(Atlas)，多个地图的集合（mspMaps），地图集会维护多个map，当系统初始化的时候，或者跟踪出现了LOST状态时，一般也会创建新的map，会生成新的地图并加入到地图集中。关于地图的合并，将在回环检测和全局优化的部分做专门介绍。 ORBVocabulary，为ORB特征的词汇表，其结构为一个KD树（k-dimensional tree），基于大量的多样的场景数据（如KITTI，EUROC，TUM RGB-D等）经过层次聚类而生成，这个词典是离线已经构建好的，直接加载使用就可以。 References