Category: Software Engineering Practices

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