ORBSLAM2系列-LocalMapping局部建图线程

跟踪线程结束后，会将关键帧通过列表传递给LocalMapping线程处理，此线程主要就是新建地图点，然后使用这些地图点和关键帧的约束，再次进行优化。注意，此线程中全部都是对关键帧的处理，没有普通帧。

处理新的关键帧

此函数主要是对新建的关键帧建立地图点和关键帧之间的联系（指的是地图点的观测、平均观测方向、深度信息和最佳描述子），以及地图点观测信息变化后需要修改的关键帧之间的联系（通过共视程度建立的关键帧之间的连接关系）

• 从mlNewKeyFrames列表前面取出一个新的关键帧mpCurrentKeyFrame，用于后面处理
• 计算该关键帧mpCurrentKeyFrame的词袋向量，用于后面新建地图点使用
• 遍历该关键帧中所有的地图点
• • 注意：这里新的关键帧中地图点是从普通帧过来的，而普通帧的地图点有两个来源：
• • • 1.经过恒速跟踪或者参考帧跟踪时，从前一帧或者参考帧进行投影匹配而来的地图点（重定位之后不能紧接着插入关键帧，可以查看Tracking线程最后关键帧插入决策）
    • 2.经过局部地图跟踪时，将局部地图中所有地图点进行投影匹配而来的地图点
  • 这里两个来源的地图点都是没有对此关键帧建立联系的（没有建立观测）
• 根据地图点对此关键帧的观测与否进行判断
• 如果没有此关键帧的观测信息，就添加该地图点与该关键帧的联系：添加观测信息、更新平均观测方向、深度信息、更新最佳描述子
• 如果有此关键帧的观测信息（单目应该进不到这里），就把此地图点放入到mlpRecentAddedMapPoints中，用于后面检查
• 建立好关键帧和地图点之间的关系，就更新关键帧之间的链接关系（按照权重，或者说是共视点数量）

剔除坏点

这部分主要是对mlpRecentAddedMapPoints列表中的地图点进行坏点检测，这个列表中不仅仅有上面处理新关键帧时添加的地图点，还有后面新建的地图点。为什么在后面新建地图点要在前面进行坏点检测？因为这个坏点检测条件都是需要使用经过一些关键帧之后才能判断的，因此，这个坏点检测放在新建地图点前面，用于下一关键帧进来时检测是没问题的。

• 已经是坏点的地图点仅仅从列表中删除（后面都有这个检测的，因此不需要从地图中删除）mlpRecentAddedMapPoints.erase(lit);
• 跟踪到该地图点的帧数比预计可观测到该地图点的帧数比例小于0.25，从地图中删除pMP->SetBadFlag();和mlpRecentAddedMapPoints.erase(lit);
• • 判断条件实际是：(mnFound/mnVisable) < 0.25 ，其中mnFound是地图点被多少帧看到（在Tracking线程中，对普通帧每个跟踪到的地图点都+1）;mnVisable 是地图点被看到次数（在Tracking线程中，不仅仅每个跟踪到的地图点都+1，而且只要是在普通帧视野范围内都+1，视野范围内并不一定会有匹配点）
• 从该地图点建立开始，到现在已经有不少于2个关键帧，但是该地图点被观测的数量不超过2个，从地图中删除pMP->SetBadFlag();和mlpRecentAddedMapPoints.erase(lit);
• 从该地图点建立开始，但现在已经过了3个关键帧却没有被剔除（认为是质量高的点，不需要继续检测了），从列表中删除mlpRecentAddedMapPoints.erase(lit);

上面删除点的方式有两种，一种是从地图中彻底删除pMP->SetBadFlag();，另一种是从列表中删除mlpRecentAddedMapPoints.erase(lit);，这两种最主要的是从地图中删除，从列表中删除也是为了以后的检测（因为遍历的地图点是这个列表中的）

新建地图点

根据视觉slam十四讲中三角化部分，新建地图点时需要通过三角化进行匹配的2D-2D点恢复成3D地图点，在此之前，Tracking线程都没有新建地图点，多仅仅是跟踪之前有的地图点，只有在这里才会出现新的地图点。

新建地图点需要两帧图像的2D点进行匹配，因此在此函数中就需要找到共视关系比较好的关键帧，然后用那些没有匹配过的2D特征点进行相互匹配（使用了BoW加速），最后再进行三角化生成新的地图点。当然，一个新的地图点出现需要经过一系列的筛选，此函数的简单流程如下：

• 寻找与当前关键帧共视关系最好的20个关键帧（共视点最多的），遍历这些关键帧，后面的所有操作都是在这个遍历下进行，也就是每个关键帧与当前关键帧进行三角化匹配（以下统称为共视关键帧和当前关键帧）
• 判断共视关键帧和当前关键帧之间的位移，如果两帧基线（两帧光心之间的距离）太小，那么就不能进行三角化计算，跳过此共视关键帧。（程序中以景深为基础，判断基线是否太小）
• • 在视觉slam十四讲中提到过，三角测量的矛盾：两帧图像在平移较大时，在相同的相机分辨率下，三角化测量将更加精确，在平移很小时，像素上的不确定性将导致较大的深度不确定性。但是平移量增大时，会导致匹配失效，这就是三角化的矛盾

• 计算共视关键帧和当前关键帧之间的基础矩阵F。为什么要求基础矩阵？因为此时经过了Tracking线程和LocalMapping线程的优化，关键帧都有了较为准确的值，那么可以使用基础矩阵设置对级约束用于之后的匹配（相当于添加了一个约束条件用于匹配）。ComputeF12函数后面单独讲解
• 使用BoW加速匹配，并用对级约束剔除误匹配的点，对共视关键帧和当前关键帧中未匹配的2D特征点进行匹配。SearchForTriangulation函数后面单独讲解
• 遍历匹配后的每一对匹配点，计算每对2D特征点之间的视差角，根据视差角判断这两帧是否能够进行三角化：
• • 视差角的计算与初始化时计算的相同（求地图点到两个光心的向量之间夹角），但是在这里没有地图点只有2D特征点，因此使用相机成像模型反推导，得到对应的向量（这个只能表示方向，因为并不知道深度多少），用这个向量之间夹角表示视差角（其实可以理解为是将2D点投影到射线上，这个射线就是光心到地图点的向量，但是并不知道地图点在这个射线的什么位置，有点像归一化平面）
  • 代码中使用了一点小技巧求解视差角：由于光心坐标在世界坐标系的坐标是 $O=t_{wc}$ ，注意下标 $t_{wc}$ 就是由相机坐标系转成世界坐标系。设地图点坐标 $P_w=R_{cw}{P}_c+t_{wc}$ ，因此向量表示 $P_w-O=R_{cw}{P}_c$
  • 注意这里计算视差角时，只关注了这个向量的方向，向量的具体数值并不考虑
• 对视差角满足要求（小于0.9998跟初始化时一样）的一对特征点进行三角化，三角化过程与初始化时一样，不讲解
• 对三角化后的3D点进行筛选：
• • 3D点是否在相机前方：转到相机坐标系下看Z轴数值
  • 将该3D点反投影到两个关键帧上，查看重投影误差
  • 3D点到两个关键帧的尺度应该差距不大：相机光心到3D点距离的比例和2D点在两个关键帧所在金字塔尺度因子的比例不应该差别太大
• 经过了筛选后，将这个3D点封装成地图点：构造MapPont、添加观测信息、向两个关键帧添加地图点、更新地图点最佳描述子、更新平均观测方向和深度信息等（跟初始化时一样）
• 将这些生成的地图放入到mlpRecentAddedMapPoints变量中，供下个关键帧进来后进行检查该地图点

ComputeF12

这个函数是用来计算两个关键帧之间的基础矩阵F。因为这时候的计算基础矩阵已经有了较准确的位姿信息R,t，因此，计算基础矩阵F很简单 $F=K^{-T}\hat{t}R{K}^{-1}$ 就能求解，但是这里需要注意的是，这里面的 $R,t$ 都指的是两个关键帧的相对变换，而正常求解的位姿信息 $R_{cw},t_{cw}$ 都是有世界坐标系（第一帧）转换的，因此这里面设计到坐标转化的问题，假设的位姿信息：当前关键帧 $R_{1w},t_{1w}$和共视关键帧 $R_{2w},t_{2w}$ ，注意这里的逻辑是反的，按照关键帧的顺序，应该是共视关键帧在前，当前关键帧在后（对极约束那个图反过来），但是在程序中设定的pKF1是当前关键帧而pKF2是共视关键帧：

$$\begin{array}{rl}{T}_{12}& =T_{1w}{T}_{2w}^{-1}\\ & =\left[\begin{array}{cc}{R}_{1w}& t_{1w}\\ o^T& 1\end{array}\right]\left[\begin{array}{cc}{R}_{2w}^T& -R_{2w}^T{t}_{2w}\\ o^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{R}_{1w}{R}_{2w}^T& -R_{1w}{R}_{2w}^T{t}_{2w}+t_{1w}\\ o^T& 1\end{array}\right]\\ & 即:R_{12}=R_{1w}{R}_{2w}^T,t_{12}=-R_{1w}{R}_{2w}^T{t}_{2w}+t_{1w}\end{array}$$

• 这里为什么要求 $F_{12}$ ？这里注意的是，初始化求解的是 $F_{21}$ 指的是由前一帧变换到后一帧的基础矩阵F，在这里求解的 $F_{12}$ 指的是共视关键帧（pKF2）变换到当前关键帧（pKF1）基础矩阵F，从关键帧的顺序上，应该是共视关键帧在前，当前关键帧在后（对极约束那个图反过来），那么为了达到跟初始化时一样的形式，就需要求解 $F_{12}$ （其实跟初始化都是一个意思，只不过代码中12的设定是反着的）

SearchForTriangulation

这个函数是两个关键帧2D特征点的匹配过程，使用了BoW加速匹配，因此整体的框架也是与前面的Tracking跟踪线程的参考关键帧跟踪中一样，只不过添加了极线约束这个条件，下面简单讲解这个流程：

• 与之前BoW加速匹配的方法一样，遍历两个关键帧中具有相同node节点的特征点
• 如果这个特征点有对应的地图点，就不参与匹配（这个三角化过程只有未匹配的2D特征点参与）
• 依然老样子求解描述子距离，选取最好的描述子距离和最佳匹配点，只不过这里筛选的条件变为极线约束：在CheckDistEpipolarLine函数中
• • 极线约束：根据 $l_2=F_{12}^T{x}_1$ 计算特征点 $x_1$ 对应到2图像的极线 $l_2$ ，再判断2图像上特征点 $x_2$ 到极线 $l_2$ 距离，是否满足阈值
  • 阈值的设定是卡方自由度为1的值3.84（为什么点到点是2自由度，而点到线是1自由度）
  • 这里需要注意的是1代表的是当前关键帧（后一帧），2代表的是共视关键帧（前一帧）
• 最后使用旋转直方图，去掉不合群的匹配点，就得到了最终的匹配点对

地图点融合

这部分主要是做些检查，基本是对局部地图应用，看看有没有一些地图点是重复的，如果地图点对应到其他关键帧中的特征点有其对应的地图点，那么这两个地图点就是重复了，需要将它们融合成一个，也就是选择了观测数目多的替换两个地图点。整个融合过程分为三个步骤，而重点在融合Fuse函数中，后面讲解：

• 正向操作：将当前关键帧的地图点向一级二级共视关键帧做匹配和融合
• 反向操作：将一级二级共视关键帧的地图点向当前关键帧做匹配和融合
• 更新地图点的信息：最佳描述子、平均观测方向和距离、关键帧之间的共视关系

Fuse

融合函数主要就是在做投影匹配（3D地图点投影到2D图像上进行匹配），然后使用网格搜索一定范围的特征点，求解最好的匹配特征点，根据这个特征点是否存在对应的地图点去判断需要替换还是添加地图点（代码中注释比较清晰明了，这里就简单说明一下）

• 遍历地图点，将3D地图点按照相机成像模型投影到关键帧中得到对应的2D点
• 根据投影得到的2D点进行网格搜索GetFeaturesInArea，得到候选匹配点
• 求解这些候选匹配点与特征点之间的描述子距离和卡方检验误差，得到最好的匹配点对关系，此时3D地图点就与2D特征点匹配上了
• 根据这个匹配关系，查看如果特征点是否有对应的地图点
• • 如果特征点有对应地图点，就选择观测数目多的替换两个地图点，这里主要是替换Replace函数，下面讲解
  • 如果特征点没有对应地图点，就添加对应的地图点，并添加观测信息

Replace

这个函数中主要是出现了两个重复的地图点需要替换，那么这里要弄清楚这个函数的两个地图点变量，准备被替换的（旧）地图点this 和准备替换的（新）地图点pMP ，这个函数要做的就是用pMP覆盖this ，总共就做了两件事：

• 更新观测到旧地图点this的关键帧信息
• • 遍历观测到旧地图点this的所有关键帧，那么就出现了两种情况
  • 该关键帧只能观测到this：用pMP去覆盖this ，并更新pMP的观测信息
  • 该关键帧不仅能观测到this还能观测到pMP：那么就直接把this删掉就好了
• 将旧地图点this的观测数据“叠加”到新地图点pMP
• • 将this的可视次数mnVisible和被找到次数mnFound直接累加到pMP中
  • 更新pMP的最佳描述子

局部BA优化

局部优化中，整理的框架与之前的g2o优化没有区别，只不过这里需要了解优化的变量：

• 当前关键帧的所有共视关键帧（一级共视关键帧），这是需要优化的变量（之前都是只优化当前关键帧）
• 一级共视关键帧的地图点，这是需要优化的变量（之前没有优化过地图点）
• 一级共视关键帧的共视关键帧（二级共视关键帧），这是不需要优化的变量，但是需要添加到g2o中，作为约束

剔除冗余关键帧

在这里有个概念就是冗余点：如果一个地图点至少被3个关键帧观测到，那么这个地图点就叫冗余点。需要剔除的关键帧就是：如果一个关键帧90%以上的有效地图点被判断为冗余，就认为这个关键帧是冗余的，需要删除该关键帧（就是这个关键帧大部分地图点都是冗余的，就不必要有这个关键帧存在了）

因此，剔除的关键帧就需要先找到每个关键帧的地图点，然后判断其地图点的观测次数，如果这个90%以上的地图点都是冗余就删除：

• 遍历当前关键帧的所有共视关键帧，然后遍历每个共视关键帧的地图点
• 遍历地图点的所有可观测关键帧，找到其观测到该点金字塔层数scaleLeveli，与共视关键帧的金字塔层数scaleLevel ，要保证观测关键帧要比共视关键帧对该地图点更加准确，因此金字塔层级需要有对应的关系：scaleLeveli<=scaleLevel+1
• 如果地图点有3个满足上面要求，并可观测的关键帧，就认为该地图点是冗余的
• 对遍历的每个共视关键帧都计算冗余地图点的数量，如果大于90%地图点数量，就删除该关键帧pKF->SetBadFlag();

SetBadFlag

关键帧的删除和地图点的删除复杂的多：

• 不能删除的情况：第0帧不能删除；mbNotErase为true的形式，就是该关键帧用于其他线程使用
• 删除与当前关键帧相连的关键帧的列表，并更新其他关键帧的这个连接列表
• 遍历当前关键帧的地图点，删除每个地图点对当前关键帧的观测信息
• 处理需要删除的该关键帧的父子关键帧：
• • 对于父关键帧，直接在父关键帧列表中删除该关键帧
  • 对于子关键帧，设置子关键帧中权重最大的关键帧作为父关键帧，如果没有找到就将需要删除的该关键帧的父关键帧（爷爷关键帧）作为父关键帧
• 从关键帧数据库中删除该关键帧

总结

到此整个LocalMapping线程就结束了，其中主要就是对局部地图进行了优化，并添加了新的地图点，这个线程中处理的都是关键帧。每个关键帧经过处理后，最后会放入mlpLoopKeyFrameQueue链表中，用于LoopClosing闭环检测线程使用。