orbslam2学习笔记 -- tracking 流程梳理
读之前提醒：首先，该文章是按照时间顺序讲述，并且不提前讲各种类关系和数据结构关系，而是在讲述系统时间执行流成的时候及时的补充相关的东西。意思就是先知道这个东西在流程中是干嘛的，然后告诉你这个东西是啥。
在slam中，rgbd，双目和单目相机的工作原理不太一样。因为单目得不到每个特征点的深度，而rgbd和双目可以得到每个特征点的深度，进而可以求解每个特征点在相机坐标系下的坐标。虽然单目可以通过三角化求得深度，但是由于极平面约束的性质，使得单目求得的位姿在尺度方面是不确定的，一般往往将其归一化处理。
这里主要讲述rgbd的流成，穿插着单目的不同。双目只是在计算深度和后端优化求解时和rgbd不太一样，其余差不多了。
在orbslam中主要有三个线程并行处理，主线程tracking负责图片的读入和vo，就是通过读入图片，提取特征点，建立初步的特征点和地图点的对应关系。从而决定是否建立新的关键帧，然后将新生成的关键帧插入localmapping线程。localmapping线程负责将局部地图里的地图点和当前帧的关键点匹配，根据匹配关系进行重投影误差的优化，进而优化当前帧的位姿。还有一个loopclose线程，这个线程主要时检测当前帧和过去所有的关键帧之间是否存在回环，也就是说是否存在当前帧恰好和之前的某个已经在地图里的关键帧看到的是同一个场景，如果是，那么就执行pose graph的优化，就是不优化地图点，只优化所有相机的位姿。
下面开始将tracking（以rgbd为例，暂时只考虑建图模式，定位模式的逻辑暂时忽略，个人觉得定位模式就是建图模式不增加地图的一个特例）
在rgbd_tum.cc中main函数进去就是一个操作，循环的读图片，然后执行函数 SLAM.TrackRGBD(imRGB,imD,tframe); 这个函数将图片信息传进去slam系统了。TrackRGBD(imRGB,imD,tframe) 这里imRGB是彩色图片，imD是视差图，tframe是时间戳。然后执行 mpTracker->GrabImageRGBD(im,depthmap,timestamp); 在这个函数内部，将rgb转换成灰度图，将视差图转换为深度图，然后构造Frame，执行track()函数。
下面开始讲说track()函数，主要以时间顺序讲述，中间会穿插各种所需要的知识。进入track()，首先看当前是否已经初始化完成，如果没有就开始进行初始化，也就是说，一般正常情况下，第一帧是开始初始化的。那么开始讲述初始化。先讲rgbd的初始化。在前面构造Frame的时候，会将图片提取金字塔，在每层金字塔上提取关键点，然后将关键点的坐标恢复到原始的图片尺寸，并且按照30*30像素的大小将图片分成grid，然后将每个关键点（也即是按照关键点的坐标），分配到每个格子中。在Frame中，记住一个比较重要，就是所有关于关键点的操作，一般都是通过关键点的索引得到的。首先用于初始化的关键点的个数应该大于500个，然后将第一帧也就是当前帧的位姿设置为单位矩阵，这里的位姿是SE3形式的。然后用这一帧构造关键帧，将关键帧添加到地图里，然后根据深度信息，将二维的图片提取的关键点的坐标根据相机模型反投影得到该点的相机三维坐标，由于第一帧位姿为单位矩阵，所以这里的相机坐标也就是世界坐标了。然后添加观测，主要是对于每个地图点来说，它可以被哪个关键帧看到，并且记录该地图点对应的关键点在该关键帧中的索引。然后计算每个地图点的描述子。这里因为每个地图点可能被很多个关键点对应，所以就有很多个描述子，这里采用一个最好的描述子，说白了就是最好的描述子距离其他所有描述子的距离和最小。然后跟新该地图点的平均观测方向和测距离范围。地图点的平均观测方向指的是，一个地图点和观测到该地图点的关键帧的相机中心的连线组成的向量，方向由相机中心指向地图点，然后将所有的这样的向量归一化之后相加，然后取平均，这就是该地图点的平均观测方向。距离范围指的是，每个地图点都有一个参考关键帧，得出该参考关键帧和该地图点的距离为一个基准，那么观测该地图点的最大距离就是那个基准距离乘以该地图点的对应的关键的所在的金字塔层对应的尺度，金字塔从0到8层，第0层尺度为1，向上依次乘以1.2。最小尺度为那个最大尺度除以1.2的7次方，也就是缩放层的层数。地图点的参考关键帧是？？？然后在地图中添加该地图点，在关键帧中添加该地图点和其对应的关键点在图片中的索引，建立当前帧特征点和地图点的对应关系。然后将该关键帧作为当前帧的参考关键帧，将该关键帧插入到localmapping里的mlNewKeyFrames.push_back(pKF)，作为后续真正通过localmapping进行一些优化操作的候选。设置跟踪状态时ok的。至此，rgbd的利用第一帧初始化完成。
接着循环读入第二帧图片相关信息，然后进入到track函数。由于我们刚进行了初始化，并且跟踪状态时ok的，所以这里直接开始做第二帧的工作。在第二帧的工作中，主要分为两部分，一部分时TrackWithMotionModel()和TrackReferenceKeyFrame()，另一部分时TrackLocalMap();。开始走流成，首先检查并更新上一帧被替换的地图点。这是干嘛呢？因为我们在这里主要想做的一个工作就是TrackWithMotionModel()，这是我们假设整个相机系统是恒速度运动的，位移和旋转都是，那么我们利用这个，当我们知道上一帧的位姿的时候，只需要用这个速度模型乘以上一帧的位姿就可以到到当前帧的初始估计位姿了。为了充分的利用上一帧的信息，也就是我们简单的想把上一帧关键点对应的地图点投影到当前帧，然后根据匹配关系利用最小化重投影误差来优化位姿。这个就是TrackWithMotionModel()要干的事，但是在localmapping线程中，由于这个线程里的一些优化，会改变地图点，这个地图点可能是该线程优化觉得是最好的，但是对于我们目前这里的需求来说可能不好，所以我们还是希望回复在localmapping线程里被更改的地图点，我们就想用上一帧原始的地图点做恒速模型的投影等。所以这里就来了这个函数，CheckReplacedInLastFrame();检查并恢复上一帧被替换的MapPoints。然后我们继续往下走。接下来就是我要说的利用恒速模型了，但是我们知道，时间流走到这里我们的系统中，只有经过了一帧，地图里也只有一个关键帧，而且这个关键帧还是我们用第一帧生成的，而形成速度模型怎么的也得用两帧的位姿信息吧，假设是两帧位姿，那么速度就是两个位姿详减呗，实际是SE3的减法。所以这里是速度模型还没有建立，怎么办。那肯定不能使用TrackWithMotionModel()了。于是我们只能使用这个了TrackReferenceKeyFrame()。这个东西是干嘛的，我理解，就是两幅图片直接的生匹配，只不过这里的生匹配也是用了一点点的技巧。好开始讲这个东西，至于TrackWithMotionModel()这个，我们会在时间流到第三帧的时候讲。进入函数TrackReferenceKeyFrame()，首先是计算当前帧的Bow向量。
那么什么是Bow向量呢？这里的计算又是怎么进行的呢？这里就不得不先解释一下图片上提取的每个关键点的描述子了。在orbslam中，提取的关键点是fast角点，使用brief描述符。fast角点简单的说就是如果一个像素，其周围连续的n的像素的亮度比其大一个阈值或者比其小一个阈值，那么这个像素便可以算是一个fast角点。另外，为了使得角点有尺度和旋转不变的特性，采用了缩放金字塔达到尺度不变的目的，采用一个从图像块的几何中心到质心的向量的角度作为其方向，这样就解决了旋转不变的问题。那么接下来就是如何描述这个图像块，也就是如何计算关键点的描述符了。其实就是事先定义好了128或者256个点对，这些点对的坐标是经过概率随机分布算好了。然后针对这个图像块，利用预先给定的图像坐标对比较坐标处的像素值大小，如果前者大就为1，否则为0，这样就形成了128bit或者256bit的向量，这就是每个关键点的描述子。然后对于一张图片，在代码中将提取的关键点的每个向量作为cv::Mat 的一行，综合起来，作为这个图片或者说是这个帧的描述子mDescriptors。那么回过来讲计算Bow向量。通过函数ComputeBoW()计算，其实就是将每一个描述子，也就是mDescriptors的每一行，计算该描述子的Bow向量和Feature向量。那么又多了连个概念，BowVector和FeatureVector。这就不得不提到词包。
我只会用最简单的例子方式讲大意，旨在让我们能明白这其中的道理，而不是非得追根于非常具体的东西上。虽然不是好读书不求甚解，但是我们是知其大意原理，而不羁绊于具体的某个东西上，至于更细节的东西，那必须靠项目锤炼靠阅读论文才能达到。
首先，这里讲词包就得提一下Loopclose，之所以要做Loopclose是因为随之slam的进行，虽然我们在不断的利用localmap进行优化，但是不可避免的误差会随着时间的增加而累计，那么研究者们就想了一个办法，就是当经过一段时间，如果slam系统能够发现自己到过同样的一个地方，那么可以通过优化前后两次的位姿，从而来整体优化slam系统的轨迹。因为我们知道，在同一个地方的位姿本来应该是尽可能的相同的，但是经过误差的累计，这两个位姿可能差别很大，那么就只能人为的告诉系统，这两个差别很大的东西其实是一个地方，为什么你们会差别很大，是因为每个关键帧的位姿计算都出现了或多或上的误差，这么所的误差累计起来以至于达到了现在的结果，那么好吧，你们开始优化，减少整体误差，也就是把在这两个帧的误差分配到所有的位姿上。这就是我们要做loopclose的原因是大意的实现。
那么现在的问题是怎么才能确定两针图片代表的场景能够表达是同一个地方呢，最简单的办法就是直接做图片做差，或者SSD，NCC之类，但是这些算法可能并不太可行，从效率和具体结果上来说不可行。那么研究者就发明了另一种方法，叫做词典。大意就是经过将图片上提取的描述子经过训练分类，分成若干类。每一个类就是一个单词，词典里的单词。那么比较两个图片是否是相似，只需要将词典索引先排放好，分别的两个图片上的描述子进行同样的方式分类，然后便能得到一个关于词典的向量，图片上有的向量处该单词的位置就为非0，图片上没有的单词，对应于向量该单词处就是0。而这个向量就是Bow向量，所以说我们可以用一个Bow向量来表示一个图片。那么我们可以通过比较这两个Bow的向量，来计算出一个分数，这个分数就表示了这两个图片的相似程度。回过来，我们继续说函数里出现的BowVector和FeatureVector。代码里的BowVector是map
现在回过来，终于兜了好大一圈终于把这个词包词典说明白了。继续接着上面讲，现在计算好了当前帧的Bow向量和Feature向量，也就是说我们将当前帧所提取的关键的对应的每个描述符，转化为一个map
至此，我们将清楚了对于系统输入的第二帧由于速度模型还没有建立，我们不得不采用比较暴力的匹配手段去匹配当前帧和当前帧的参考关键帧的关键点，从而根据重投影误差优化计算初步的当前帧的位姿，这个函数是TrackReferenceKeyFrame()。接着将当前最新的参考关键帧作为当前第二帧的参考关键帧
接着开始执行TrackLocalMap();这个函数主要讲述的就是将一个局部地图里的地图点，经过之前两帧之间的匹配和优化的出来的位姿作为初始位姿，将地图点投影到当前帧，利用优化重投影误差来优化当前帧的位姿。由于这里的地图点，不仅限于上一帧或者是上一个参考关键帧的地图点，但是绝对是已经被关键帧观测到的地图点。我们相信经过更多的地图的优化，当前帧的位姿会更加的准确。执行函数UpdateLocalMap();，更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints。在该函数中，主要执行了两个函数，分别是 UpdateLocalKeyFrames()和UpdateLocalPoints()。我们分别来说这两个函数。首先更新局部关键帧，遍历当前帧的MapPoints，将所有能观测到当前帧MapPoints的关键帧添加到局部地图里。并且将这些关键帧中与当前帧共同看到地图点个数最多的关键帧当做当前帧的参考关键帧。将上面添加的关键帧，也就是能够看到当前帧的地图点的所有关键帧，对于每个这样的关键帧，将与其共视程度最好的前10个关键帧添加到局部地图里。这里的共视程度就是共同看到的地图点的个数。并且将这个关键帧的所有子关键帧，父关键帧也加入到局部地图里。一个关键帧的父关键帧就是和自己共视程度最高的那个关键帧，共视程度最高就是可以共同观测到地图点个数最多。同时，如果确定了一个帧的父关键帧，那么这个关键帧就是这个父关键帧的子关键帧了。一个关键帧只有一个父关键帧，但是可以有很多个子关键帧。接下来解释UpdateLocalPoints()，在该函数里，把现在局部地图的所有关键帧能够看到的所有有效地图点都加入到局部地图里。这样就执行完了UpdateLocalMap();。然后执行函数SearchLocalPoints();进入该函数，首先我们先标记那些已经被当前帧看到的地图点，这些点不参与后面的判断是否在可以投影到当前帧视野里，并且将这些点的被观测次数加1。然后将局部地图里的所有地图点，投影到当前帧，如果可以投影到当前帧，则增加观测次数。之后执行函数SearchByProjection(mCurrentFrame, mvpLocalMapPoints, th)。该函数的主要作用就是寻找局部地图里的点和当前帧的点的匹配情况。首先遍历局部地图里的所有地图点，去掉跳过以下三类点。
// a 已经和当前帧经过匹配（TrackReferenceKeyFrame，TrackWithMotionModel）但在优化过程中认为是外点
// b 已经和当前帧经过匹配且为内点，这类点也不需要再进行投影
// c 不在当前相机视野中的点（即未通过isInFrustum判断）
根据该地图点利用初始位姿投影到当前帧的图像坐标和利用地图点的金字塔层级确定的搜索范围半径，得出当前关键帧图片上属于搜索窗口内的关键点的索引。然后用这个地图点的描述子，和这个搜索窗口内的所有关键点的描述子一一匹配，确定最优的那个关键点，作为这个地图点的正确的匹配。并且当描述子匹配的距离大于一定的阈值的情况下，为当前的帧的关键点添加其对应的的地图点。接下来利用局部地图的地图点和当前帧的匹配关系，利用优化重投影误差执行函数Optimizer::PoseOptimization(&mCurrentFrame);来优化当前帧的位姿。接下来更新当前帧的地图点IncreaseFound()，另外统计在当前帧的地图点里，能够被其他关键帧观测到的点的总个数，这个也叫做局部地图跟踪的程度效果。比如，如果在当前关键帧中的地图点被其他关键帧观测到的总个数小于30，那么就说局部地图跟踪失败了。好了，至此TrackLocalMap将完了。
好了，现在回到track函数。接下来按照时间流继续走。如果TrackLocalMap函数执行成功了，我们就可以根据当前帧的位姿和上一帧的位姿，构造出速度模型了。就是简单的拿当前帧的位姿减去上一帧的位姿，这里的减法是SE3减法。接下来就是删掉VO的匹配的点。遍历当前帧的所有地图点，如果该地图点没有被任何一个关键帧观测过，那么就删除当前帧的这个关键点和地图点的匹配关系。接着走是清除mlpTemporalPoints，这个临时的地图点事在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd），由于我们在第二帧并没有使用速度模型跟踪，所以目前这个临时地图点事空的。
接下来判断是否需要生成一个新的关键帧。首先需要一个新的关键帧需要满足一个必须的条件，就是当前帧中的地图点被关键帧观测到的个数与当前帧所有深度有效的地图点的个数的比例要比较低，这就代表了当前帧和关键帧里的相似程度不是特别大。在这个条件下，如果，很长时间没有加入关键帧，或者LocalMapper处于空闲状态，或者跟踪快要失败，这三个情况任意一个成立，那么就可以加入关键帧了。
接下来就是用当前帧生成新的关键帧，并且将tracking线程的参考关键帧也更新为目前新生成的这个关键帧。并且，将当前帧的参考关键帧也更新为这个新生成的关键帧。将当前帧中，深度在一定阈值之内的点包装成地图点，加入到地图中，并且添加该地图点和关键帧的联系，对该地图点添加关键帧观测次数，更新该地图点的描述子，观测方向等。这里需要注意一下代码的联系和一些细节了，因为tracking线程的一些操作会影响到localmapping的一些行为。我们需要注意，在tracklocalmap的时候，地图点全部是与当前帧有关联的关键帧所能够观测到的地图点，然后将这些地图点投影到当前帧，建立当前帧关键点和局部地图中的地图点的联系，但是这时候我们并没有建立地图点和当前帧关键点的联系，说白了就是我们只知道当前帧的某个关键点对应局部地图里的哪个地图点，但是从那个地图点的角度出发，我们并不知道这个地图点也被当前帧观测到来了。这样做是合理的，因为地图点与关键点的联系只发生在地图点和被设置为关键帧的关键点的联系，因为即使这里是局部地图里的地图点，但是这些地图点也是全局地图里的地图点的引用或者指针等，总之是共享一个内存的东西。那么问题来了，如果当前帧的情况达到了生成新的关键帧的条件，而在关键帧生成的过程中我们又会将深度值小于一定阈值的关键点包装成新的地图点，添加到地图里，从而也建立地图点和关键点之间的联系。需要注意深度小于一定阈值的关键点其实分为三类，一类是自己还没有与任何地图点产生关联，第二类是自己有匹配的地图点，但是这个地图点没有被任何一个关键帧所观测到过，第三类是自己匹配了局部地图里的地图点，而这些地图点全部都是至少被一个关键帧观测到过。而这里的新包装生成的地图点只是前两类，这两类也会加入地图点与关键帧的联系以及关键帧和地图点的联系，和加入到地图里。至于第三类匹配，如果当前帧恰好是生成关键帧了，这三类点只是建立了关键点与地图点的联系，然后在localmapping线程里会建立地图点与关键帧的联系。然后就是在全局地图里加入该地图点。并且也将当前帧的关键点和新建立的地图点建立联系。将该关键帧加入mpLocalMapper线程。然后去掉在当前帧中那些经过优化后显示为outlier的匹配关系。至此，slam系统track()函数的第二帧也讲完了。
时间继续走，我们系统收到了第三帧图片信息。进入track()函数。经过了初始化，并且目前跟踪状态也是正常。那么直接就开始利用速度模型进行帧间跟踪了。执行函数TrackWithMotionModel(); 执行函数UpdateLastFrame();主要做的是对于双目或rgbd摄像头，根据深度值为上一关键帧生成新的MapPoints。因为在跟踪过程中需要将当前帧与上一帧进行特征点匹配，将上一帧的MapPoints投影到当前帧可以缩小匹配范围，由于在之前的跟踪过程中，去除outlier的MapPoint，如果不及时增加MapPoint会逐渐减少。将上一帧中，关键点深度在一定阈值以内的点包装成地图点，并且建立这些地图点和上一帧关键点的联系，然后把所有新生成的地图点添加到一个临时的容器里mlpTemporalPoints，后面会全部删除。将速度模型乘以上一帧的位姿作为当前帧的初始位姿。接下来通过函数 SearchByProjection(mCurrentFrame, mLastFrame, th, mSensor == System::MONOCULAR)来寻找上一帧的地图点和当前帧的关键点之间的匹配关系。在这个函数中，依次将上一帧的地图点，投影到当前帧中，根据地图点所在金字塔层级，确定一个搜索窗口范围，在该范围内的所有关键点和这个地图点的描述子进行计算距离，为这个地图点选取距离最小的当前帧的关键点作为匹配。如果整个计算下来的匹配点较少，则扩大窗口的搜索半径再计算匹配一次。然后优化当前帧的位姿，根据当前关键点与地图点的匹配，根据最小化重投影误差，优化当前帧的位姿。然后剔除优化时计算出来的outlier的匹配联系。然后就和上一段之后的情况一样，为当前帧跟新参考关键帧，然后TrackLocalmap优化当前帧的位姿。跟新速度模型，删除上一帧中添加的临时地图点。检测是否需要关键帧等。然后剔除当前帧中的outlier的匹配。
在之后的第四帧以及以后，都上以上的流程。当然没有讲述跟踪失败，重定位等。也没有讲述单目相机的情况。这里做个说明，单目由于初始化等比较复杂，会单独拿出来。至于跟踪失败重定位等，后面串完三个线程的大概流程后，会简单的说一下，因为需要用到loopclose等，然后和跟踪有很多相似的地方。
好了，至此，tracking讲完了。