ORB-SLAM2阅读笔记

    从RGB-D相机获取帧，对输入的图片进行预处理，寻找特征点， 进行特征匹配用BA来做最小化重投影误差，进行跟踪和定位每帧的相机（即算出R t）。运用局部BA算法设置局部地图并优化（当前帧与地图中的3D路标点进行匹配来估计相机位姿），

一、预处理：

    从RGB相机提取出ORB特征（具有尺度旋转不变性），生成立体坐标，得到特征点。

    ORB特征： FAST关键点，例如FAST-9 FAST-12，只需要比较一个像素与邻域像素的灰度差别大不大。但是原始的FAST角点经常会出现“扎堆”现象，所以在第一遍检查后要进行非极大值抑制，在一定区域内仅仅保留响应极大值的角点。在ORB中对FAST进行改进，指定最终提取的角点数量N，对原始角点算Harris值后选取前N个角点最为最终角点集合。另外，构建图像金字塔并在每一层上面检测角点（分辨率越高角点数量越多）来保持尺度不变性，用灰度质心法来描述旋转。提取了Oriented FAST关键点后，对每个点用改进的BRIEF描述子计算，使得描述子具有平移、旋转和缩放的变换下仍然具有良好的表现。然后用“汉明距离“描述子距离描述两个特征点之间的相似程度，特征点数量很大，因此用FLANN快速近似最近邻算法匹配。

    补充1：用改进的ORB做特征点匹配后，得到不同帧之间的特征点对，用RANSAC五点法求取本质矩阵F，进而得到相机的位姿R t。五点法的具体思想是：如果采用最小二乘法，难免因为一些误匹配而导致会有较大的偏差，RANSAC的思想就是先随机选取5个点，计算出本质矩阵F，然后检测其他的点是否满足本质矩阵，得到满足该本质矩阵的个数（内点），进而进行迭代，得到内点最多的那个本质矩阵即为所求。五点算法不受点集共面的影响。

二、跟踪：

    主要思路是在当前帧和局部地图之间找出更多的对应关系，以此来优化当前帧的位姿。

    跟踪里面的三种运动模型解释，这三种模型都是用来得到当前帧的位姿T的。

    (1)MotionModel

        恒速运动模型，假设运动是匀速的，根据上一帧的跟踪结果得到一个运动速度，v*t就可以估算当前帧的位姿

    (2)RefrenceModel

        根据匹配后两帧之间的约束关系来求解位姿估计

    (3)RelocalizationModel

        在跟丢了以后，把当前帧放到local map中去，找到在database中与当前帧最相似的那一帧(类似于loop closing)。先选出一些候选关键帧，然后将候选关键帧一个个与当前帧的BoW去匹配，不行就抛弃，最后用PnP去求解当前帧与关键帧的位姿，并优化，优化不行的也抛弃。

        局部地图跟踪：前面已经得到了对位姿的估计，通过投影可以得到更多的对应关系，与current frame 有共同点的局部地图中关键帧序列为K1，在covisibility graph中与K1相邻的称为k2。loacl map 中有一个帧（属于K1），它和current frame共同观测到的点最多。针对k1  k2可见的每一个地图点云，通过如下步骤，在current frame中搜索：

        a.将 地图点投影到当前帧上，如果超出图像范围，就将其舍弃；

       b. 计算当前视线方向向量v与地图点云平均视线方向向量n的夹角，舍弃n·v < cos(60°)的点云； 

        c. 计算地图点到相机中心的距离d，认为[dmin, dmax]是尺度不变的区域，若d不在这个区域，就将其舍弃； 

        d. 计算图像的尺度因子，为d/dmin；

        e . 将地图点的特征描述子D与还未匹配上的ORB特征进行比较，根据前面的尺度因子，找到最佳匹配。

        关键帧的选择，可以根据时间间隔或者特征点数目的阈值来判断是否插入新的关键帧。

        关键帧的标准：

              a. 在上一个全局重定位后，又过了20帧； 

            b.局部建图闲置，或在上一个关键帧插入后，又过了20帧； 

            c.当前帧跟踪到大于50个点； 

            d.当前帧跟踪到的比参考关键帧少90%。

三、局部建图：

    Local Mapping这部分 包括插入关键帧，剔除冗余的地图点和关键帧，还有进行局部集束调整。

    (1)关键帧的插入：将新的关键帧ki插入covibility graph中，并且更新与其它共享地图点的关键帧节点相连接的边，同时更新了Ki的生长树，并且计算了表示关键帧的词袋BOW。

    (2)当前地图点的剔除：要想创建地图点，在创建点云的时候前三帧测试通过约束才能被保存，这样才会保证可跟踪且在三角化的时候不会出现较大误差。一个点要想被加入map中，需要满足：

    a.在可预测能够观测到该点的关键帧中，有超过25%的关键帧能跟踪到这个点；

    b. 如果一个地图点被构建，它必须被超过三个关键帧观察到。

    一旦一个地图点被创建了，只有在被少于三个关键帧观测到时才会被剔除，剔除是在local BA或者剔除关键帧的时候才发生。

    (3)新的地图点创建：通过将检测到的ORB特征点，找到Covisibility Graph中与之相连的关键帧Kc，进行特征匹配，然后将匹配到的特征点进行三角化。 ORB特征点对三角化后，检查正向景深、视差、反投影误差和尺度一致性，这时才得到地图点。一 个地图点是通过两个关键帧观察到的，而它也可以投影到与之相连的其他关键帧中，这个时候可以使用Tracking部分的跟踪局部地图来在附近的关键帧中找到匹配。

    (4)Local BA： 局部集束调整（local BA）会将当前处理的关键帧Ki进行优化，优化时如下图所示：现在优化Pos3位置的关键帧。同时参与优化的还有：     

    所有在Covibility Graph中与该关键帧相连的关键帧Kc，即下图中的Pos2； 

    所有被这些关键帧观察到的地图点，即X1和X2。 

    另外还有能观察到地图点的但并未与当前处理的关键帧相连的关键帧，即下图中的Pos1。 

    但要注意的是，诸如Pos1的关键帧，参与优化中的约束，但不作为变量去改变它们的值。优化时得到的外点会在优化的中期或后期被剔除。

                                 

           (5)局部关键帧剔除：随着关键帧数目增长后，BA复杂度越高，除非视觉改变了，否则关键帧的数目在相同环境中不应该无休止地增长。 如果说一个帧中90%的点都能被超过三个关键帧观测到，则认为这个帧是一个冗余帧，将其删除。

四、 闭环检测：

          主要思路是检查队列中关键帧，检查满足闭环条件，计算相似变换（SE3），校正闭环，进行闭环融合和图优化。

             （1）闭环条件检测：计算关键帧ki与 Covisibility Graph中与其相连的关键帧之间词袋的相似程度，本文用离线训练了大量的词袋，因此只需要将ki中的词袋与训练的词袋做比较，将闭环检测转变成了类似于模式识别的问题。

                LoopClosing中的关键帧都是LocalMapping中送过来的：送过来一帧，就检查一帧。

                    // in LocalMapping::Run()

                    mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

                    // in LoopClosing.cpp

                    void LoopClosing::InsertKeyFrame(KeyFrame *pKF)

                    {

                        unique_lock lock(mMutexLoopQueue);

                        if(pKF->mnId!=0)

                       mlpLoopKeyFrameQueue.push_back(pKF);

                    }

                     因此需要在KeyFrameDataBase中寻找与mlpLoopKeyFrameQueue相似的闭环候选帧

                 DetectLoop()

                      在共视关系中找到与当前关键帧Bow匹配最低得分minScore，在除去当前帧共视关系的关键帧数据库中，检测闭环候选帧：                   

              vector vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);                   

                      闭环候选帧筛选过程：

                         1. Bow得分>minScore；

                         2. 统计满足1的关键帧中有共同单词最多的单词数maxcommonwords；

                         3. 筛选出共同单词数大于mincommons(=0.8*maxcommons)的关键帧；

                         4. 相连的关键帧分为一组，计算组得分（总分），得到最大总分bestAccScore，筛选出总分大于minScoreToRetain(=0.75*bestAccScore)的组，用组中得分最高的候选帧lAccScoreAndMatch代表该组。计算组得分的目的是剔除单独一帧得分较高，但是没有共视关键帧，作为闭环来说不够鲁棒。

                          对于通过了闭环检测的关键帧，还需要通过连续性检测（连续三帧都通过上面的筛选），才能作为闭环候选帧。 这一步相当于是检测上一步得到的关键帧集是否是真的可以使用的. 连续性检测的意思就是，是否我们在三个当前的关键帧内都同时发现了某一个闭环候选帧的话，那么就表明当前的SLAM系统已经闭环。

    比方说，在上图中，通过数据库查询，我们可以在A点得到闭环候选关键帧有两个(1,2)。在下一次进入DetectLoop函数的时候，我们当前拿到的关键帧是B，那么在B点我们可以得到的闭环候选关键帧是(1,2,3)，以此类推，在再下一次进入DetectLoop函数的时候，也就是在C点的时候，我们这时对比到的闭环候选关键帧是(2,3)，所以2这个闭环候选关键帧被检测到了三次。在LoopClosing中，mnCovisibilityConsistencyTh = 3 一致性共视阈值被设为3，并且如果一旦有一个闭环候选关键帧被检测到3次，系统就认为检测到闭环。

　（2）计算Sim3： 上一步已经找出了闭环帧，接下来就是进行后端优化。computesim3主要是为了找出当前帧和闭环帧之间找到更多的对应点，通过这些点计算当前帧与闭环帧之间的sim3变换，求解出R t s，这个过程总共进行了三次对应点的查找工作。

                    （a）对每一个闭环帧都与当前帧进行bow匹配，如果闭环帧与当前帧的匹配点个数少于20个，则丢弃，否则构造一个sim3求解器并保存。这一步主要是针对效果较好的闭环帧构建sim3求解器。

                    （b）对上一步得到的每一个满足条件的闭环帧通过RANSAC迭代，求解sim3。

                    （c）通过返回的sim3再次进行匹配点查找，本次查找的匹配点数量，会在原来的基础上有所增加。

                                  sim3计算得到了R t s，通过SearchBySim3()函数来进行区域匹配，找到步骤a中漏掉的匹配点.具体过程如下：

                                  1）通过Sim3就能知道相机1和相机2之间相互变换的变换矩阵 

                               2）通过vpMatched12更新vbAlreadyMatched1和 vbAlreadyMatched2，如果 vpMatched12[i]非0的话，则说明这个点已经是一个匹配点vbAlreadyMatched1[i]=true;获取这个mappoints在pKF2中的像素点idx2，将vbAlreadyMatched2[idx2]=true

                                3）通过Sim变换，确定pKF1的特征点在pKF2中的大致区域（通过坐标变换把pKF1的mappoints变换到相机2坐标系下，再转成像素点(u,v)），根据半径确定搜索区域，遍历该区域内的所有特征点，与 (u,v)算描述子，从而根据阈值就能判断是否有新的匹配

                                  4）类似于上一步 通过Sim变换，确定pKF1的特征点在pKF2中的大致区域，找出符合条件的新的匹配点对。

                    （d）在得到第二次匹配的匹配点之后，利用非线性优化的方法去优化sim3，得到更精确的R t s。

                   （e）将闭环关键帧和它所有的相连关键帧所能看到的mappoints全部恢复出来，尽可能得到当前关键帧所能看到的所有的地图点，为下一步做投影匹配做准备。

                    （f）使用投影得到更多的匹配点，如果匹配点数量充足（大于等于40个），则接收闭环。

                 由于单目相机匹配后存在尺度不确定性问题，所以，单目SLAM是一个sim3相似变换群，有7个自由度，即1个尺度因子，3个旋转角度，3个位移。双目或RGB-D中s=1，有6个自由度。

              

         （3）闭环融合：闭环矫正的第一步就是融合重复的点云，并且在 Covisibility Graph中插入新的边连接闭环。根据相似变换矫正当前位姿，与其相连的所有关键帧也会被矫正，闭环处关键帧观测到的地图点都会被映射到一个小范围里，然后去搜索它的近邻匹配。这样可以有效的进行数据融合，更新关键帧位姿，以及在图中的边。

          （4）Essenrial Graph优化：为了有效的完成闭环，使用本质图去优化位姿图，这样把闭环误差分散到整个图中。

Ps：1. Covisibility graph和Essential graph

    Covisibility Graph是一个无向图模型，顶点表示关键帧，顶点之间边的权值表示两个关键帧观测到的地图点的个数。边的权值大小能够反映关键帧之间视图的相似程度，主要是为了在local mapping ,local BA ,以及tracking的时候能够保持near constant time（否则每次重新project和优化所有点的话整个系统会越来越慢）。

    为了在优化阶段减小计算量，提出了  Essential graph的概念，这个能够连接所有的node，但是edge会减少很多，可以认为是Covisibility Graph的最小生成树（MST）。 Essential graph主要是在loop closing的时候用来distribute error的，比如找到KFm和KFn之间累积的drift是M，就顺着 essential graph把这个M均匀分布掉。