Category: ICT-Information and Communication Technology
-
SLAM系列之ORB-SLAM3开源系统代码解析-地图回环及合并检测
这篇文章将向大家介绍回环和地图合并检测相关实现逻辑，和局部建图器类似，回环检测实现的类为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
-
SLAM系列之ORB-SLAM3开源系统代码解析-局部建图器(LocalMapping)
本篇文章将向大家介绍局部建图器的代码逻辑，局部建图器的类名为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
-
SLAM系列之ORB-SLAM3开源系统代码解析-mono(+imu)tracking
这篇文章将向大家介绍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
-
SLAM系列之ORB-SLAM3开源系统代码解析-基本数据结构简介
感谢读者的关注，近期准备向大家介绍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