ORB_SLAM2源码解析-LocalMapping建图

ORBSLAM三大线程之LocalMapping部分。

1.概述

1.1 图的概念

Convisibility Graph

顶点：所有pose；边：pose-pose有共视关系(共视点大于15个)

换言之，将所有共视关系大于15的pose点连接起来，连接的edge的权重就是共视点的个数。图中绿色即为edge，红色为局部地图点，红色+黑色为全局地图点。

Spanning Tree

图论的概念，无向图能产生不同的生成树(spanning tree)，通过边能遍历所有节点。而所有边的权重加起来最小的生成树就是最小生成树。

这里的spanning tree是在无向图convisibility graph的基础上，以共视点最多为筛选标准形成的最小生成树。这里面同时还包含了 loop closure edges(红色)

Essential Graph

包含了spanning tree以及convisibility graph中具有极佳共视关系(>100)的边

1.2 总体结构

最后剔除冗余关键帧的条件有两个：

1. 90%以上的MapPoints能被至少3个其他关键帧观测到。
2. 他们都处在同一尺度下

2. 处理关键帧ProcessNewKeyFrame

处理要达成四个目的：

• 计算该关键帧特征点的Bow映射关系
• 处理新匹配上的MapPoints
• 更新关键帧间的连接关系
• 将该关键帧插入到地图中

2.1 处理过程

LocalMapping中有一个成员变量std::list mlNewKeyFrames是等待处理的关键帧列表。在tracking线程中，调用了void LocalMapping::InsertKeyFrame(KeyFrame *pKF)函数，向mlNewKeyFrames链表插入了元素。因此在处理关键帧前，要首先调用bool LocalMapping::CheckNewKeyFrames()检查等待关键帧链表是否为空。

处理时调用void LocalMapping::ProcessNewKeyFrame()，步骤如下:

1. 从链表取出一帧

   分三步：锁线程，取元素，删除顶部元素

2. 计算关键帧特征点Bow映射关系

3. MapPoints和当前关键帧绑定

   MapPoints分为两种情况：在tracking过程跟踪到的MapPoints；创建关键帧时创建的MapPoints。前者很可靠，直接为他们添加属性（添加观测者，平均观测方向和观测距离范围，更新最佳描述子），后者不可靠需要放到mlpRecentAddedMapPoints等待进一步检测。

4. 更新关键帧间的连接关系

   包括Covisibility图和生成树

5. 插入关键帧到地图

2.2 更新连接关系UpdateConnections

更新的目的是更新Covisibility图和生成树。步骤如下：

1. 统计共视关系

   共视关系保存在map KFcounter中，这个成员key是关键帧，value是权重（权重为其它关键帧与当前关键帧共视3d点的个数)，结构如下：

   

   计算的方法借助了std::map mObservations，它记录了观测到该MapPoint的KF和该MapPoint在KF中的索引，是MapPoint的成员变量。

   

   因为MapPoint是属于当前帧的，所以只需要执行KFcounter[obervation->first]++;就可以统计和当前关键帧共视的其他关键帧，以及共视点个数。

2. 记录有良好共视关系的关键帧

   搜索KFcounter中共视点个数大于阈值th(15)的关键帧，将他们放入vector > vPairs容器中（将关键帧的权重写在前面，关键帧写在后面方便后面排序）。同时记录有最多共视点的关键帧，如果没有发现任何大于阈值的帧，就用最多共视点关键帧建立连接。

   记录完后sort排序，按共视点个数从大到小。

3. 更新图连接

   更新与该关键帧连接的关键帧与权重mConnectedKeyFrameWeights = KFcounter;，将排序的vPairs拆开，容器mvpOrderedConnectedKeyFrames记录排序后的关键帧，容器mvOrderedWeights记录排序后的共视点个数（权重）

   然后更新生成树，父节点为共视程度最高的那个关键帧mpParent = mvpOrderedConnectedKeyFrames.front();，并且父节点将本帧添加为子节点(建立双向连接关系)mpParent->AddChild(this);

3. 检查剔除地图点MapPointCulling

对上一函数获取到的最新加入的局部地图点(创建关键帧时创建的MapPoints)mlpRecentAddedMapPoints进行检查，该地图点被创建后的三个关键帧里必须要经过严格的测试，这样保证其能被正确的跟踪和三角化。

删除过程调用SetBadFlag()干两件事情：删除点与帧的观测关系mObservations.clear()，删除帧与点的对应关系

for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        pKF->EraseMapPointMatch(mit->second);// 告诉可以观测到该MapPoint的KeyFrame，该MapPoint被删了
    }

for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        pKF->EraseMapPointMatch(mit->second);// 告诉可以观测到该MapPoint的KeyFrame，该MapPoint被删了
    }

Copy

删除条件如下:

1. 已经是坏点

   pMP->isBad()==true

2. IncreaseFound / IncreaseVisible < 25%

   在SearchLocalMap（TrackLocalMap第二步），通过isInFrustum判断，就调用IncreaseVisible()。在TrackLocalMap中，通过优化，得到了内点和外点，如果点不是外点，说明这个点不仅能被观测，还能和特征点对应上，调用IncreaseFound()。
   跟踪到该MapPoint的Frame数相比可观测到该MapPoint的Frame数的比例需大于25%

3. 小于观测阈值

   从该点建立开始，到现在已经过了不小于2个关键帧，且观测数pMP->Observations()<=cnThObs，其中cnThObs是针对不同类型相机设置的阈值单目为2，双目为3。

如果这个点已经过了3个关键帧而没有被剔除，则认为是质量高的点，因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测。

4. 创建地图点CreateNewMapPoints

4.1 创建地图点一般步骤

1. 找到共视程度最高的n帧

   寻找与当前关键帧拥有最多共视点的n帧，n取值与相机有关，单目要求会高一些。

2. 遍历共视关键帧

   1. 如果有新关键帧需要处理就处理新的

   2. 判断关键帧间距是否足够长

      对于双目而言，间距需要大于双目相机本身的基线；对于单目而言，间距与场景深度中值之比小于0.01

   3. 特征匹配

      通过极线约束限制匹配时的搜索范围，进行特征点匹配。所谓极线约束就是说同一个点在两幅图像上的映射，已知左图映射点PLPL，那么右图映射点PRPR 一定在相对于PLPL的极线上，这样可以减少待匹配的点数量。最后得到的匹配结果放在vector > vMatchedIndices;中。

      

   4. 三角化生成地图点

4.2 三角化

一般来说单目相机没有深度信息，需要依靠三角化获得，双目相机能自己获得深度信息。如果点太远，用相机模型获取深度就不是很合适。

如图所示，如果zz的值很大，则视差dd就会变得很小，误差的影响就非常大，所以这时候同样也需要三角化计算深度。

由相似三角形可知：

$$z=\frac{fb}{d}, \ d=u_l-u_r$$

三角化解法：

假设一个点在三维空间的坐标是$P=[X,Y,Z,1]^T$，其中$p,p’$是匹配好的特征点，他们在不同位姿相机坐标系下:

$$sp=TP\\ s'p'=T'P$$

其中$T$可以表示为：

$$T=\begin{bmatrix} t_1&t_2&t_3&t_4&\\ t_5&t_6&t_7&t_8&\\ t_9&t_{10}&t_{11}&t_{12}&\\ \end{bmatrix} =\begin{bmatrix} \mathbf{t}_{0[4\times1]}\\ \mathbf{t}_{1[4\times1]}\\ \mathbf{t}_{2[4\times1]} \end{bmatrix}$$

左边叉乘$p_1,p_2$($z$已被归一化为1)：

$$\begin{bmatrix} y\mathbf{t}_2-\mathbf{t}_1\\ \mathbf{t}_0-x\mathbf{t}_2\\ x\mathbf{t}_1-y\mathbf{t}_0\\ \end{bmatrix}P=0$$

将两个式子合并（第三个等式没有有效的约束）：

$$AP=\begin{bmatrix} y\mathbf{t}_2-\mathbf{t}_1\\ \mathbf{t}_0-x\mathbf{t}_2\\ y'\mathbf{t}_2-\mathbf{t}_1\\ \mathbf{t}_0-x'\mathbf{t}_2\\ \end{bmatrix}P=0$$

四个有效方程，P中含有三个未知数，这是一个超定方程，对矩阵AA奇异值分解SVD求最小二乘解。

具体步骤如下：

1. 提取匹配特征点

   从vMatchedIndices中提取

2. 计算视差角

   视差角需要计算两个：第一个是当前帧和参考帧的视差夹角，第二个是双目相机左镜头和右镜头的视差夹角。

   对于帧与帧的视差角，利用匹配点反投影可以得到方向向量，利用直线夹角计算公式$\theta=\frac{\vec{n_1}\vec{n_2}}{|\vec{n_1}||\vec{n_1}|}$即可算得。对于左右镜头视差角利用近似的几何关系算得。$\theta=\arctan(\frac{1}{2}B/Z)$

   

3. 计算3D点

   单目相机采用三角化的办法，双目相机如果物点距离很远也采用三角化，距离很近直接采用深度（判断距离远近比较两个视差角）

4. 检查3D点

   检测生成的3D点是否在相机前方，检测重投影误差是否在可接受范围，检查尺度连续性

5. 将3D点构造成MapPoint

   老生常谈了，调用构造函数，然后添加属性

6. 加入检测队列

   这些MapPoints都会经过MapPointCulling函数的检验

5. 融合地图点SearchInNeighbors

前面通过新加入的关键帧建立了一些地图点，然而这些地图点和以前建立的可能有重复，所以这一步需要剔除重复。

步骤:

1. 获得最佳共视关系的邻接关键帧

   找到当前帧一级相邻与二级相邻关键帧。一级邻接是指和当前帧有最佳共视关系的那些帧，数量根据单双目情况，有所不同。二级关键帧是和一级关键帧有良好共视关系的（程序中是最好的5帧）

2. 正向融合

   融合时有两个参数：帧，地图点。若存在重复，则删除；若不重复，添加属性成为正式的地图点，当然也要检查是否具有匹配关系。正向融合的帧是一二级邻接关键帧（遍历），地图点是当前关键帧产生的地图点。

3. 反向融合

   反向融合帧是当前关键帧，地图点是所有一二级关键帧对应的地图点（遍历）。

4. 更新

   更新当前帧MapPoints的最佳描述子，平均深度，平均观测主方向等属性。同时更新covisibility图，更新当前帧的MapPoints后更新与其它帧的连接关系。