在之前的很多博客中，我们从不同角度分析了ORB-SLAM3的相关内容。这篇博客，我们从双目Tracking这个角度，进一步详细分析ORB-SLAM2/3是如何实现的。以此为例，也可以了解ORB-SLAM系统里到底是如何实现帧间(上一帧和当前帧)的特征提取与匹配的。因为ORB-SLAM2和ORB-SLAM3在双目模式下流程基本是相同的，所以这里我们以ORB-SLAM2的代码进行分析。这样可以省去一些繁杂的分支判断等与核心双目Tracking不相关的内容，更方便厘清脉络。
1.系统启动
我们以ORB-SLAM2中的stereo_euroc为例对双目Tracking进行分析。再复杂的程序都从Main()函数中开始，ORB-SLAM也不例外。
1.1 数据预加载
ORB-SLAM在stereo_euroc.cc文件中定义了LoadImages()函数，该函数用于根据指定的文件夹和时间戳文件，构造双目影像的路径，用于之后的影像内容加载。该函数在Main()函数的一开始就被调用，如下图所示。
1.2 系统对象构建
然后，在Main()函数中又新建了System类的名为SLAM的对象，如下图所示。
这行代码其实调用的是System类的构造函数，在构造函数中，对系统进行了一系列的初始化操作，包括ORB字典的加载、Tracking、LocalMapping、LoopClosing、Viewer线程的创建与启动等，如下图所示。
完成之后，至此系统就完成创建和初始化了。
这里需要稍微注意一下的是，在Tracking的构造函数中，加载了我们传给Main()函数的配置文件里的诸多参数，如相机的内参、ORB相关参数等，如下图所示。
至此，系统正式工作前的准备工作就进行完了，Main()函数也执行到了下面这一行。
下面就要开始真正的逐帧输入系统进行Tracking了。
2.第一帧影像传入
然后，在Main()函数中，通过for循环读取每一帧影像，并调用System类的成员函数TrackStereo()进行逐帧Tracking。
比如，此时第一帧的双目影像输入了TrackStereo()函数。进一步，在这个函数中，它又会调用Tracking类的GrabImageStereo()函数，将传入的双目影像和时间戳给它。这个函数会对当前帧进行位姿估计，并返回结果，如下所示。
进一步，在GrabImageStereo()函数中，首先会根据传入的影像新建系统能够识别的Frame对象，然后调用Tracking类的Track()成员函数进行位姿估计。
2.1 Frame对象创建
在系统中重载了Frame类的构造函数，使其可以应对不同的数据输入。对于双目而言，构造函数中最重要的是两件事情：分别在左右影像上提取ORB特征，以及计算双目匹配点对，如下所示。
2.1.1 ORB特征提取
对于ORB特征提取，是通过调用Frame类的成员函数ExtractORB()实现的。这里为了加速提取，还使用了多线程技术。具体来说就是左右影像分别开两个线程进行特征提取。而ExtractORB()函数又是调用ORBextractor类重载的括号运算符实现的，如下所示。
这里因为侧重点的关系就不再对此继续进行展开，感兴趣的话可以参考之前的这篇博客，专门分析了ORB特征的提取。
2.1.2 双目特征匹配
在左右影像都提取到特征以后，就要进行双目匹配了。具体而言，是通过Frame的成员函数ComputeStereoMatches()实现的。简单来说就是根据左影像上的特征点去右影像上搜索。找到匹配以后，将右影像上对应点的横坐标存储在mvuRight，计算的深度存储在mvDepth。默认情况下，它们的值都为-1，如下图所示。
同理，受限于篇幅，这里不再赘述，感兴趣可以参考之前的这篇博客，里面有更详细的内容。
在完成了上面的步骤以后，系统此时就运行到了这里，如下。
下面就要开始进入正式的双目Tracking阶段了。
2.2 开始双目Tracking(双目初始化)
进入Track()函数后，根据状态不同有两个大的分支：没有初始化(NOT_INITIALIZED)的分支和已经初始化的分支。如果对于ORB-SLAM系统中的状态切换还不太熟悉，可以参考这篇博客的内容。前面说了，由于此时我们才输入第一帧影像，系统还没有初始化，所以会进没有初始化的分支，如下所示。
可以看到，系统主要做了一件事情，那就是调用StereoInitialization()双目初始化函数尝试进行初始化，如下。
在初始化函数中，系统会判断当前帧包含的特征点个数是否大于阈值。如果满足条件则继续尝试初始化。这其中包含一系列步骤：设置当前帧的位姿为原点、将当前帧转换为关键帧并插入地图、将当前帧中成功双目匹配的特征点(判断条件是利用双目计算的深度大于0)添加到地图中并与该关键帧关联、设置上一帧为当前帧等。
当这些步骤都做完了，双目初始化也就顺利完成了，此时Track()函数执行的未初始化分支也就结束了，退出Track()函数。接着，如同套娃一般的，退出GrabImageStereo()函数、TrackStereo()函数，直到Main()函数的for循环，如下图所示。
这样第一次循环就执行结束了。下面就是输入第二帧以及之后帧的流程了。
3.第二帧影像传入
当第二帧影像传入时，还是会调用SLAM.TrackStereo()函数，进而调用GrabImageStereo()函数，将第二帧的影像转换成系统能识别的Frame对象(在Frame构造函数中提取特征、双目匹配)。到这个地方，都和第一帧是完全一模一样的。然后又进到Track()函数。从这个地方开始有所区别。前面也说了，整个Track()函数根据系统是否初始化包含两个重要的分支。在第一帧中，我们走完了未初始化的分支并且成功初始化，此时系统状态被设为OK。所以当第二帧输入Track()函数的时候，就会走我们刚刚没走的另一个分支了。而整个分支也是Tracking最终要的分支，如下图所示。
在整个Tracking阶段，其实主要有两层:帧间Tracking和局部地图(local map)Tracking，分别对应TrackWithMotionModel()和TrackReferenceKeyframe()、TrackLocalMap()。下面分别介绍。
3.1 帧间Tracking(TrackReferenceKeyFrame())
根据当前系统的状态(速度为空或者当前帧ID小于上次重定位帧ID+2)，如果满足条件，会调用TrackReferenceKeyframe()，否则调用TrackWithMotionModel()进行位姿估计，如下图所示。这两个函数最终都会返回跟踪是否成功bOK。
对于我们第二帧输入的影像而言，此时Tracking::mVelocity是为空的。因为这个成员变量除了在Tracking.h中声明了以后，还没有被赋初值。我们可以找到Track()函数中对其赋值的地方，如下。
可以看到其实是在很后面的，而我们现在其实一遍都没有运行过这些代码，所以这里的第一个条件就满足了。而至于第二个条件，是不满足的。因为帧的ID是从1开始累加的，第一帧影像的ID为1，此时第二帧影像的ID为2。而mnLastRelocFrameId变量初值为0，所以这里的判断是2<0+2?是不成立的。但不管怎么说，第一个条件满足了，所以当第二帧进来Track()函数的时候，会调用TrackReferenceKeyFrame()进行帧间位姿估计。
在这个函数中，我们会估计当前帧和参考关键帧之间的位姿变换关系并进行位姿优化，如下所示。
那么这里参考关键帧是什么呢？答案是我们输入的第一帧。如果你观察够仔细的话，可以看到在双目初始化函数StereoInitialization()中的后面有这么一行mpReferenceKF=pKFini。正是通过这一行，将我们根据输入的第一帧Frame构造的pKFini关键帧赋给了mpReferenceKF。所以我们也可以说，当第二帧输入的时候其实进行的就是第一帧和第二帧之间的匹配。
3.1.1 BoW计算
首先会计算当前帧的BoW向量。我们可以进入这个函数看看，如下。
首先，函数将Frame类的cv::Mat类型的mDescriptors转换成了vector<cv::Mat>类型的描述向量vCurrentDesc。当然这里需要注意的是，在mDescriptors中，每一行代表一个特征点的描述向量，所以这个矩阵有多少行就表示有多少特征点，有多少列则表示描述子有多少维。所以这个转换操作其实非常简单，我们也可以点进去看一下，如下。
就是把一个大的二维矩阵按照行一行行的拆开了。每一行作为vector的一个item。
那么这个mDescriptors在哪里被赋值的呢？如果你还有印象的话，前面说了，在Frame构造函数中调用ExtractORB()函数提取了ORB特征，正是在这个函数中将mDescriptors“填满”了，如下。
而且可以看到，不仅如此，对于双目的左右影像而言，我们可以同时获取左右影像的两个描述矩阵mDescriptors和mDescriptorsRight，虽然我们没有使用右目的描述矩阵罢了。当然，这里也可以得出一个结论，那么就是：对于双目影像而言，我们只是用左目影像提取的特征计算词袋，不使用右目的数据。
最后，在ComputeBoW()函数中，调用DBoW库的函数transform()，将一系列的ORB描述子转换为BoW向量和特征向量，如下图所示。
3.1.2 特征匹配
然后会对参考关键帧和当前帧进行特征匹配，会返回匹配点对的个数。如果小于一定的阈值，即认为TrackRferenceKeyFrame()函数位姿估计失败并返回。否则就继续进行。
这里需要注意的是，在ORB-SLAM里面，其实并非所有匹配都是直接拿两帧的描述子直接进行匹配的，还有一些是通过地图点获得的描述子。我们可以看看ORBmatcher.cc里面以Search开头的函数，SearchByProjection()、SearchForInitialization()、SearchForTriangulation()、SearchBySim3()这些函数要么是直接通过Frame类的mDescriptor获取描述子，或者是通过MapPoint类的成员函数GetDescriptor()获取某个地图点的描述子。然后调用DescriptorDistance()计算差异。比如我们以SearchByProjection()函数的某个重载为例，展示如下。
当然，这里面还有个相对细节的问题。地图点MapPoint的描述子从何而来呢？如果一个双目相机，左右影像上都有这个地图点，我是用左目的描述子还是右目的呢？如果这个地图点被多帧F1、F2观测到，我是用F1的描述子还是F2的呢？这是个好问题。
我们不着急回答这个问题。先回答，对于两帧影像而言，进行帧间匹配的时候用的是哪个描述子。比如我们以SearchForInitialization()为例，他计算描述子距离是这样的，如下。
看到这个图，其实这个问题就已经有答案了：他用的都是左目的描述子。因为右目的描述子叫做mDescriptorsRight。当然了，SearchForInitialization()这个函数只用在单目初始化，双目的时候就不会调用它，自然也不存在左右目的问题。但我们还是能够体会这种思想的。
那么再回到地图点描述子的问题。在地图点中，有个GetDescriptor()的成员函数，他会直接返回mDescriptor，如下。
那么mDescriptor在哪里被赋值呢？事实上，除了构造函数中被赋值之外，最终要的就是在MapPoint::ComputeDistinctiveDescriptors()中被赋值了，如下。
从名字也就知道，这个函数就是用来从多个描述子中选择一个最优的赋给当前地图点。选择的范围和标准是什么呢？首先说选择的范围，看下面的代码：
可以看到，其实就是该地图点的观测。换句话说就是我们会寻找能观测到该地图点的所有关键帧，然后从它们那里获取描述子。比如说某个地图点被5个关键帧观测到了，这样它就有5个备选的描述子。而且还可以看到，我们只用了关键帧中左目影像的描述子(mDescriptors)，而非右目(mDescriptorsRight)。
这样第一个关于选择范围的问题就回答了。第二个问题是选择标准呢？这么多个描述子，选择哪个最具有代表性呢？还是看代码。
可以看到，首先计算了待选描述子之间的距离。然后我们的选择标准是：Take the descriptor with least median distance to the rest.翻译成中文就是，选择距离所有剩余描述子距离中值最小的那个描述子。这样就十分明朗了。举个例子，比如我们有D1、D2、D3、D4，4个描述子。现在D1到D2、D3、D4的距离分别是5、3、2；D2到D1、D3、D4的距离分别是5、6、8；D3到D1、D2、D4的距离分别是3、6、2；D4到D1、D2、D3的距离分别是2、8、2。建议在纸上拿笔画一下，就十分清楚了。对距离排序取中值可以得到：D1距离中值为3、D2为6、D3为3、D4为2。所以我们认为D4最优代表性，将其作为该地图点的描述子。
明白了这个之后，那么在哪里调用了MapPoint::ComputeDistinctiveDescriptors()函数呢？事实上有很多地方都调用了，如下所示。
而这里和我们紧密有关的就是双目初始化函数StereoInitialization()函数，在前面也已经说过了。
在创建地图点的时候，就将描述子赋值好了。而且到目前为止，我们只输入了一帧影像，所以其实地图点也没得选，只能老老实实用第一帧左目中特征点的描述子。
好了，前面说了这么多，回到我们的主线上来。在TrackWithReferenceKeyFrame()函数中调用的是SearchByBow()。不论怎么说，它还是获取了描述子，然后计算距离，如下所示。
3.1.3 位姿赋值
经过上面一系列的匹配，我们就获得了参考关键帧和当前帧之间匹配点对的个数。事实上，在系统中并没有直接根据2D-2D的特征匹配估计位姿，而是先将上一帧的位姿赋给当前帧，作为位姿优化的初值，如下。
3.1.4 位姿优化
如果一切顺利，到了这里，我们就会对当前帧的位姿进行一次优化。这个优化我们固定地图点，以上一帧位姿作为初值开始优化，只优化当前帧位姿。这样我们就能得到相对准确的当前帧的位姿。
3.1.5 外点删除
最终，根据优化的结果，我们可以得到一些外点，我们将这些外点删除。并且我们做一个最终的判断，看剔除外点后还剩多少点，如果还剩大于10个点的话，即认为TrackRferenceKeyFrame()函数位姿估计成功。
运行到这里，TrackReferenceKeyFrame()函数就结束了，返回状态为True。对这个函数感兴趣还可以可以参考这篇博客，里面也有比较详细的介绍。
3.2 局部地图Tracking(TrackLocalMap())
此时，程序运行到了Track()函数的这里。
由于TrackReferenceKeyFrame()函数成功，所以bOK为True，然后经过一系列判断，就会进入局部地图Tracking，也就是TrackLocalMap()函数，如下。
在这个函数中，简单来说我们会尝试将当前帧的地图点与局部地图进行匹配，如果成功，对位姿再进行进一步优化，如下。
这里留意一下局部地图跟踪成功的判断条件，如果匹配点对个数小于50，就认为失败了。这里你可能会有问题？什么才算局部地图呢？在后面会有解答。
函数首先调用UpdateLocalMap()进行局部地图更新，主要包含两部分：局部关键帧更新和局部地图点更新。下面分别介绍。
3.2.1 局部关键帧更新
对应UpdateLocalKeyFrames()函数，如下。
其实策略是比较容易理解的。首先，我们遍历当前帧中左目影像上的所有特征，如果它有对应的地图点，我们就调用地图点的GetObservations()函数，获取直接观测到该点的其它关键帧。除此之外，我们还将和已经添加的关键帧的相邻关键帧也添加进来。当然了，这里也有个判断，如果此时局部关键帧个数已经大于80个了，就不添加了。
3.2.2 局部地图点更新
对应UpdateLocalPoints()函数，如下。
简单来说就是遍历刚刚得到的局部关键帧vector(mvpLocalKeyFrames)，依次遍历这些关键帧关联的地图点，检查是不是OK，如果没问题的话，就将该地图点添加到mvpLocalMapPoints中。
事实上，我们在跑ORB-SLAM中看到的红色地图点，其实就是这里的mvpLocalMapPoints。在UpdateLocalMap()函数中第一行就将当前的局部地图点通过SetSetReferenceMapPoints()函数传给了Map类的成员变量mvpReferenceMapPoints，如下。
之后，在MapDrawer的DrawMapPoints()函数中就会获取到这些点，然后用红色绘制出来，如下所示。
3.2.3 位姿优化
最后，类似的，还会进行依次位姿优化。根据优化的结果把一些外点给去除掉。如果剩余的地图点大于50，就认为局部地图跟踪成功。
至此，系统状态设为OK，运行到了Track()函数的这里：
3.3 剩余步骤
之后，在Track()函数中还会做一些收尾工作，比如更新运动模型的速度mVelocity，如下所示。
判断是否需要插入新的关键帧。
而如果Tracking失败并且地图中关键帧个数小于5个，则会重置系统。
最后，把当前帧(mCurrentFrame)变为上一帧(mLastFrame)。
这样，我们第二帧影像输入就顺利走完Track()函数了。之后类似的，就是套娃一般的，退出GrabImageStereo()函数、TrackStereo()函数，直到Main()函数的for循环。
4.第三帧影像传入
当第三帧影像传入时，还是会调用SLAM.TrackStereo()函数，进而调用GrabImageStereo()函数，将第三帧的影像转换成系统能识别的Frame对象(在Frame构造函数中提取特征、双目匹配)。到这个地方，都和第二帧是完全一模一样的。然后又进到Track()函数。由于系统是OK状态，还是进入OK状态的分支，尝试利用TrackReferenceKeyFrame()或者TrackWithMotionModel()进行Tracking。这个地方和第二帧不同的是：在第二帧的时候mVelocity为空的。而经过我们上面3.3部分的处理，此时就不为空了，而且当前帧ID也大于2。所以顺利成章的就使用TrackWithMotionModel()进行Tracking，如下所示。
4.1 帧间Tracking(TrackWithMotionModel())
函数内容与TrackReferenceKeyFrame()类似，如下。
但不同的是这个函数是将当前帧和上一帧进行匹配，而TrackReferenceKeyFrame()是将当前帧和参考关键帧进行匹配。然后我们可以先看是否成功的条件。可以看到，如果帧间匹配个数小于20，或者经过优化以后剩余的地图点个数小于10，即认为Tracking失败。
4.1.1 上一帧更新
对应UpdateLastFrame()函数，如下所示。
具体而言，首先会尝试获取上一帧的参考关键帧mpReferenceKF，然后会从Tracking::mlRelativeFramePoses中取出最后一个元素，赋给Tlr。所以，这里Tlr表达的意义就很明确了：lastframe和referenceKeyFrame之间的Transformation。第二个问题是，mlRelativeFramePoses存储的是什么东西？它是Tracking类的成员变量。从字面意思来看，它存储的是帧间的相对位姿。我们可以找到使用它的地方，如下。
可以看到，在Track()函数的最后部分，根据Tracking的不同情况，对mlRelativeFramePoses进行了赋值。如果Tracking顺利的话，就会获取当前帧的位姿mTcw以及当前帧参考关键帧位姿的逆，让它们相乘，得到的就是当前帧和参考关键帧之间的相对位姿Tcr。我们就会把它放到mlRelativeFramePoses里面。而如果Tracking有问题，我们还是会增加mlRelativeFramePoses的元素，但是是直接将最后一个元素再复制一遍放进来。
回到主线上来。现在我们传入了第三帧影像，运行到了UpdateLastFrame()。我们直接获取mlRelativeFramePoses的最后一个元素，如下图所示。
根据上面的介绍，那么此时，这个最后一个元素表示的就是第二帧相对于参考关键帧的相对位姿。所以，这里对应的变量名就叫做Tlr。
这样，我们再将上一帧相对于参考关键帧的位姿Tlr和其参考关键帧的位姿相乘，得到的就是上一帧相对于整个坐标系的位姿。这一步做完，我们就认为对于上一帧位姿的更新就完成了。
事实上，函数除了对于上一帧的位姿进行了更新，对于其关联的地图点也进行了更新。简单来说，首先对上一帧关联的地图点根据深度进行了排序(默认为升序)。然后优先选择深度小于阈值的点进行创建。而如果所有深度小于阈值的点个数小于100，就只好放宽点要求，选择深度最小的100个点。这个逻辑也就可以回答为什么有些时候感觉明明地图中某些点的深度显然超过了阈值，却依然还保留的问题。当然，你也可能会好奇，这个深度阈值是怎么算出来的。如果你有印象的话，对于双目而言，我们在配置文件中指定了一个叫做ThDepth的属性。这个属性可以理解为双目基线的倍数，也就是说，在双目基线长度的ThDepth倍数内，我们就认为是近点，否则就是远点。进一步，系统在读取了ThDepth倍数以后，会在Tracking的构造函数中利用它计算出真正的深度阈值，也就是我们这里用到的mThDepth，如下。
到这里，对于上一帧更新的操作基本就完成了。
4.1.2 初始位姿赋值
然后，系统会根据更新以后的上一帧的位姿，结合运动模型，估计位姿，并将其赋给当前帧，作为后续优化的初始值。
这里所谓的运动模型，其实就是在上一帧位姿的基础上乘以速度，就可以得到当前位姿。
4.1.3 帧间匹配
这样，TrackWithMotionModel()函数就运行到帧间匹配了，也就是用SearchByProjection()函数，返回匹配点对的个数。这里有个值得注意的地方，帧间匹配匹配的是什么？是上一帧左目的特征和当前帧左目的特征吗？答案是否定的。准确来说是上一帧关联的地图点和当前帧地图点之间的匹配。如何匹配呢，是通过地图点的描述子进行的。这些操作都在SearchByProjection()中进行，如下图所示。
在这个函数中，我们首先取到上一帧的特征点，遍历所有的特征点看看有没有对应的地图点。如果有的话，就获取当前地图点以及它对应的描述子。然后我们利用地图点的三位坐标解算在当前帧中可能的范围。我们获取到这个局部范围内所有的特征，然后逐个遍历，看看是否有对应的地图点。如果该特征点有对应地图点，那就获取到描述子。最终将这两个描述子进行比较，计算相似性，如下。
最终，将匹配的个数返回。
4.1.4 位姿优化
然后，如果匹配的点对个数小于20，使用宽松的阈值再试一次。如果还是小于20的话，那就认为Tracking失败。如果一切顺利的话，就开始调用PoseOptimization()位姿优化。
4.1.5 外点剔除
这一步和3.1.5部分是一样的。如果剔除外点后剩余的点个数大于10的话，就认为Tracking成功。
至此，我们的帧间Tracking就完成了，状态返回为True。
4.2 局部地图Tracking(TrackLocalMap())
然后我们就进入了局部地图跟踪。这部分和3.2是一样的，此处就不再赘述。
4.3 剩余步骤
这一部分和3.3部分也是一样的，此处也不再赘述。
至此，我们第三帧影像输入就顺利走完Track()函数了，系统状态为OK。之后类似的，就是套娃一般的，退出GrabImageStereo()函数、TrackStereo()函数，直到Main()函数的for循环。
5.第n帧影像输入
现在，其实我们已经基本走完可能的路径了。当第n帧输入进来的时候，其实和第三帧是一样的。还是会调用SLAM.TrackStereo()函数，进而调用GrabImageStereo()函数，将第n帧的影像转换成系统能识别的Frame对象(在Frame构造函数中提取特征、双目匹配)。然后又进到Track()函数。由于系统是OK状态，还是进入OK状态的分支，尝试利用TrackReferenceKeyFrame()或者TrackWithMotionModel()进行Tracking。和第三帧一样的，由于mVelocity不为空，而且当前帧ID也大于2，所以顺利成章的就使用TrackWithMotionModel()进行Tracking。
这样，不出意外的话，每一帧都会重复上面的过程。以上便是我们对于ORB-SLAM2双目模式Tracking的分析。而ORB-SLAM3其实和这个流程是基本类似的。
本文作者原创,未经许可不得转载,谢谢配合
