ORB-SLAM3源码阅读笔记3:Frame(KeyFrame)与MapPoint的关系解析

Jul 30,2021   7186 words   26 min

Tags: SLAM

1.概述

在ORB-SLAM3中,Frame/KeyFrame在很大程度上是和MapPoint有直接关系的。正是在正确Tracking了Frame之后,才会生成MapPoint。这种密切联系可以体现在两个方面,一是在Frame中有专门的成员变量mvpMapPoints用于存放当前帧生成的MapPoint,如下所示。 另一方面,对于每个MapPoint而言,也有专门的mObservations成员变量来记录其对应的Frame。只不过对于MapPoint而言,相关联的Frame叫“观测”,如下图所示。 所以从这里也可以看出,Frame和MapPoint是一种多对多的关系一个Frame可以有多个MapPoint,同时一个MapPoint也可以被多个Frame观测到,如下图所示。 对于MP1,有KF1、KF2和KF3观测到了它,而另一方面,KF1除了观测到MP1,也观测到了MP2和MP3。

2.Frame与MapPoint的创建

2.1 Frame的创建

Frame类是整个ORB-SLSM3非常核心的类之一。输入的影像最后都会被转换成Frame或KeyFrame的对象。具体而言对于输入的影像,其首先在System类的TrackStereo()成员函数将传入的影像传给Tracking类的成员函数GrabImageStereo()。在这个函数中,通过调用Frame类的构造函数,新建Frame类的对象mCurrentFrame,它是Tracking类的public成员变量,之后会在其它函数中被直接调用。它是Tracking类中非常重要的成员变量之一,很多变量都直接从它获取或者赋值给它,例如估计的当前帧位姿。 当然,由于有多种传感器,所以重载了多个Frame的构造函数。

2.2 MapPoint的创建

MapPoint作为ORB-SLAM3里一个基础且相对抽象的数据类型,并不像Frame一样有对应的特定输入数据(影像)。根据SLAM的运行逻辑,地图点正是在初步估计位姿(Tracking)之后,才会生成。所以对于ORB-SLAM3而言,最初的MapPoint是在Tracking类中的Track()成员函数中创建的。进一步来说,这里又可以分为两部分一部分是初始化阶段的地图点建立,另一部分是正常Tracking时的地图点建立。在介绍具体创建流程之前,我们会首先对MapPoint本身进行一些简单的介绍。

(1) MapPoint类型简介

MapPoint是ORB-SLAM3中一个基础类型,用于表示根据二维特征点计算出来的三维地图点。MapPoint除了有坐标描述子等基本属性外,还有一个比较重要的概念就是“观测”。所谓观测,在ORB-SLAM3中就是一个键值对键指的是观测到该地图点的关键帧,而值指的是该地图点在该帧中对应的特征的索引,如下图所示。 我们有三个地图点(MP1、MP2、MP3)以及三个关键帧(KF1、KF2、KF3)。对于MP1而言,其有KF1和KF2可以观测到该点。在KF1中MP1对应的特征点序号为1,在KF2中MP1对应的特征点序号也为1。所以MP1一共有两个观测:<KF1,1>和<KF2,1>。同理,我们可以得到MP2、MP3的观测如下:

  • MP2:<KF1,4>,<KF2,2>,<KF3,2>
  • MP3:<KF2,4>,<KF3,3>

而且,根据这种观测关系,我们也可以很容易的在不同帧的特征点之间建立关联,形成匹配关系。例如MP1在KF1和KF2中同时被观测到了,分别对应KF1中的特征1和KF2中的特征1。那么显然,KF1中的特征1和KF2中的特征1就是同一个点,就可以自然地建立这两个特征之间的对应关系。甚至,可以找到多帧之间的对应关系,如KF1中的特征4、KF2中的特征2、KF3中的特征2都是对应MP2。当然,这里需要明确的一点是,不是所有的特征点都能生成对应的地图点(有些特征可能质量不好就被丢弃了),但每个地图点都有且至少有一个对应的特征点(否则也不可能生成地图点)

(2) 初始化阶段的创建

前面提到在Track()函数中调用了各类初始化函数,如双目初始化函数StereoInitialization()、单目初始化函数MonocularInitialization()。我们以双目初始化为例,介绍MapPoint最初的创建过程。 对于双目初始化而言,首先就会将当前的普通帧Frame类对象mCurrentFrame变成初始化关键帧KeyFrame类对象pKFini。然后,将初始关键帧插入地图集中。并且根据不同的相机模型进入不同分支,如下图所示。 对于常规的相机模型,我们逐个遍历Frame类的mCurrentFrame对象中的N个特征点,根据条件判断是否将其添加为地图点。这里我们详细介绍一下地图点的添加流程,如下。 首先我们获取到第i个特征点对应的深度mvDepth[i]。如果深度为正,则认为是正确的地图点,否则就什么都不做。然后,我们调用Frame类的成员函数UnprojectStereo(),如下所示。 通过索引,获取对应的u、v像素坐标,然后按照公式计算其对应三维坐标的x、y分量,将2D的特征像素坐标转化为相机坐标系下的3D坐标的x、y分量。然后,我们基于计算好的3D坐标x、y分量、初始关键帧pKFini、以及当前地图指针,即可以新建一个MapPoint对象pNewMP。这里调用的是MapPoint众多构造函数中的一个,如下。 可以看到,首先我们传入的位置Pos被赋给了MapPoint的成员变量mWorldPos。而传入的关键帧则按需赋给对应变量。传入的地图指针也同理赋给相应变量。新建好MapPoint地图点以后,显然就是要把这个地图点与帧关联起来,让它们彼此“绑定”。这就需要分别调用MapPoint的成员函数AddObservation()和KeyFrame的成员函数AddMapPoint()。首先是调用AddObservation()函数添加观测,可以看到,这里传入的两个参数分别是当前的初始关键帧pKFini以及特征点的索引i。而至于AddObservation()里做了什么,其实也非常简单,如下所示。 在上面也说了,所谓观测,是通过map形式储存的。所以这里就首先看当前观测里有没有传入的关键帧,如果有了就不做修改,否则就添加。而对于AddMapPoint(),我们传入MapPoint对象pNewMP,以及其对应的特征点索引i。函数内容如下。 做的事情也是很容易理解的。通过传入的索引idx将对应的MapPoint赋给mvpMapPoints。通过以上两个步骤,就可以建立MapPoint与Frame/KeyFrame之间的双向联系。之后我们还要调用MapPoint的成员函数ComputeDistinctiveDescriptors()计算该地图点的描述子、UpdateNormalAndDepth()更新该点的法向与深度信息。然后,再通过AddMapPoint()函数将建立的地图点与地图相关联起来。最后,我们还要将这个新的地图点添加到当前帧mCurrentFrame的mvpMapPoints中。注意这里不是上面提到的初始关键帧pKFini。而且另外需要注意的是,对于Frame类而言,并没有KeyFrame中的AddMapPoint()函数方便直接调用,所以这里就只能手动添加了,虽然做的事情是一样的就是稍微麻烦点。StereoInitialization()函数运行到这里结束后,系统就会输出一句“New Map created with xxx points”,也就说明地图点初始化成功了。

在添加好地图点之后,StereoInitialization()函数继续又做了一些其它事情,虽然看起来与MapPoint没有直接关系,但其实会间接影响到后面的LocalMapping,而LocalMapping是会直接影响到后续地图点的创建的。所以这里也简单介绍一下,如下所示。 其中一步就是将初始关键帧pKFini通过LocalMapping的成员函数InsertKeyFrame()传给LocalMapping线程。 简单来说就是将传入的pKF放到LocalMapping的成员变量mlNewKeyFrames中。而在之前也说过,LocalMapping线程会循环查询mlNewKeyFrames的状态,只要其长度不为0,就开始尝试进行LocalMapping。而在StereoInitilization()剩下的步骤就是就是将当前帧赋给mnLastKeyFrame,以用于其它步骤。将当前系统状态设为OK。

(3) Tracking阶段的创建

在Tracking阶段,一般使用的两个函数是TrackWithMotionModel()TrackReferenceKeyFrame()。当然,还有一个TrackLocalMap()函数用于对Tracking结果进行进一步优化。

TrackReferenceKeyFrame()

在TrackReferenceKeyFrame()函数中,在一开始就对当前的Frame地图点进行了操作,如下。 在函数中,通过ORBMatcher将参考关键帧包含的地图点与当前帧的地图点进行匹配(词袋模型),返回的是vpMapPointMatches。其包含的是系统认为匹配的地图点。SearchByBow()函数如下所示。 可以看到,函数首先调用KeyFrame的成员函数GetMapPointMatches()函数获得其关联的地图点,而这个函数其实就是直接返回了KeyFrame的成员变量mvpMapPoints,如下所示。 然后,函数将传入的vpMapPointMatches全部置为NULL,个数等于Frame帧的特征个数N。然后下面就是一系列的计算度量值进行匹配的操作,如果符合条件,就将对应的MapPoint对象赋给vpMapPointMatches。所以对于SearchByBow()函数而言,其返回值vpMapPointMatches中,只有认为是匹配的地图点才会有MapPoint指针,否则都为NULL。而如果匹配的地图点个数小于15,就认为Tracking失败,直接返回False。否则就将vpMapPointMatches的内容赋给mCurrentFrame.mvpMapPoints。并且将当前帧的位姿设为上帧的位姿(仅仅只是作为位姿优化的初始值),然后就执行PoseOptimization()开始优化。这里需要注意的一点是,在PoseOptimization()函数中,修改了mCurrentFrame.mvbOutliers这个vector,它的长度和mCurrent.mvpMapPoints相同,用于表示某个地图点是不是为Outlier,如下图所示。如果当前地图点是Outlier,就被赋为True,否则就为False。 而mCurrentFrame.mvbOutliers在Frame的构造函数中被赋了初始值,全为False,如下所示。 在TrackReferenceKeyFrame()函数中,优化完成之后,再次遍历地图点,把一些mvbOutliers中被设置为外点的MapPoint丢弃,如下所示。 最后,完成对当前帧相对于ReferenceKeyFame的Tracking,退出函数。这便是TrackReferenceKeyFrame()函数中对于mCurrentFrame.mvpMapPoints的访问与修改。

TrackWithMotionModel()

TrackWithMotionModel()函数同样是用于Tracking。 这里与TrackReferenceKeyFrame()函数不同的是,这里如果有IMU,就根据IMU进行初始状态估计,然后将其付给当前帧mCurrentFrame,如下图所示。 而这里调用的PredictStateIMU()的核心就是根据IMU的观测,算出初始位姿,并直接赋给mCurrentFrame,如下图所示。 下一步需要注意的是,将当前帧mCurrentFrame的地图点列表mvpMapPoints全部清空了,或者说把每个元素都赋为NULL(用空指针填充mvpMapPoints)。这一步其实是与上面介绍的TrackReferenceKeyFrame()里调用SearchByBoW()函数中将MapPoint列表的每个元素赋值为NULL是异曲同工的。然后,类似的,在清空mCurrentFrame. mvpMapPoints以后,调用SearchByProjection()尝试将上一帧mLastFrame的地图点投影到当前帧中进行匹配并返回匹配到的个数。对于获得的地图点个数,如果小于20,就放宽阈值,重新进行搜索。而如果这样还不行就直接退出函数,如下图所示。 这一步执行完以后,类似的,调用PoseOptimization()函数对当前帧的位姿进行优化。后续步骤就是和TrackReferenceKeyFrame()相同的。

TrackLocalMap()

除了前面提到的两个Tracking函数,如果说当前已经有一些地图点了,那么我们还会在上面两个函数跟踪的基础上进一步利用局部地图再Tracking一下,对估计的位姿进行进一步的优化。在这里也涉及到一些和MapPoint相关的操作。关于TrackLocalMap()更详细的说明可以参考TrackLocalMap函数文档。这里只对其中涉及到MapPoint操作的部分进行介绍。简单来说,运行TrackLocalMap()函数的前提是已经有了一些局部地图点以及相关联的局部关键帧。在Tracking类中,分别用mvpLocalMapPointsmvpLocalKeyFrames表示。类似的,在TrackLocalMap()函数中,会对当前帧的位姿进行进一步优化。考虑到不同传感器,所以设计了不含IMU包含IMU的两个分支,如下图所示。 对于不含IMU的情况,还是使用和上面两个函数相同的PoseOptimization()函数进行优化;对于包含IMU的情况,则使用新的函数。这里针对普通Frame和KeyFame又分别调用了不同的函数。这三个函数的详细说明见TrackLocalMap函数文档。

(4) LocalMapping线程中的创建

在LocalMapping线程中,同样会对当前帧的MapPoint进行修改。具体而言,在Run()函数中对传入LocalMapping的当前帧进行处理。与地图点相关的主要部分如下。 而且在LocalMapping中,也会进行局部BA同时优化地图点与关键帧位姿,如下图所示。 LocalInertialBA()针对有IMU的情况,LocalBundleAdjustment()用于没有IMU的情况。这和之前在三个Track函数(TrackReferenceKeyFrame()、TrackWithMotionModel()、TrackLocalMap())中介绍的位姿优化(PoseOptimization()、PoseInertialOptimizationLastFrame()、PoseInertialOptimizationLastKeyFrame())是不同的,它们只优化帧位姿而不优化地图点。可以简单记忆为PoseOptimization()相关函数都是只优化位姿,不优化地图点,都是在Tracking线程(主线程)中被调用的。而LocalBA()相关函数同时优化位姿和地图点,都是在LocalMapping线程中被调用的

3.Tracking、LocalMapping线程与MapPoint的关系

对于Tracking线程中的三个Track函数(TrackReferenceKeyFrame()、TrackWithMotionModel()、TrackLocalMap())而言,其主要目的在于估计Frame位姿,解算每一帧的地图点MapPoint,并放在各自帧的成员变量mvpMapPoints中。然后,固定解算的地图点,只对估计的帧位姿进行优化,得到较为精确的位姿。其主要作用是将Frame类与MapPoint类关联起来,将2D的特征点转换成3D坐标。这个时候新建的地图点还没有与Map或LocalMap有任何关联。更准确来说,在Tracking的Track()函数中调用了StereoInitialization(),在这个函数中新建了地图点,并将它们与Map进行了关联。而对于三个Track函数,就没有进行关联了。

在LocalMapping线程中,通过取出输入帧里所包含的MapPoint进行局部建图,如果满足条件,将地图点放到LocalMapping的成员变量mvpLocalMapPoints中,构建局部地图LocalMap,关联的关键帧放到mvpLocalKeyFrames中。并在LocalMapping完成之后进行一次整体的局部优化,同时优化局部地图点坐标与关联的关键帧位姿。所以,其主要作用是将MapPoint与Map或LocalMap关联起来

本文作者原创,未经许可不得转载,谢谢配合

返回顶部