一起学ORBSLAM2（11）ORBSLAM的localmapping

转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125

ORBSLAM的局部建图线程实际做的工作是来维护全局map以及管理关键帧的，对tracking得到的关键帧进行筛选融合，以及对关键帧中的地图点进行融合，剔除冗余的关键帧和地图点，维护一个稳定的全局map，并将筛选后的关键帧提供给回环检测使用。

局部建图线程主函数流程：

1 设置当前不能接受新的关键帧到系统中，即在局部建图（处理关键帧）过程中不允许新关键帧集有变动

2 对新关键帧进行处理。建立新的关键帧及其属性（计算关键帧BOW向量，将关键帧的地图点加入该关键帧，并更新地图点的属性（观测到该地图点的关键帧，该地图点的平均观测方向，最优描述子））并加入全局map

3 检测当前关键帧对应的地图点是否是好的（筛选条件:地图点是否是好的,地图点的查找率大于0.25, 该地图点第一关键帧(第一次观察到该地图点的帧id)与当前帧id相隔距离, 该关键点的被观察到的次数）

4对极约束检测特征点，并建立地图点加入全局map。利用对极约束对当前帧和共视关键帧（当前帧的）进行三角测量（匹配当前帧和关键帧，然后对每对匹配通过三角化生成3D点，之后进行检验（检验地图点在两帧中的深度，在两帧中的重投影误差，尺度连续性）），如果满足对极约束则建立当前帧的地图点及其属性（a.观测到该MapPoint的关键帧  b.该MapPoint的描述子  c.该MapPoint的平均观测方向和深度范围），将地图点加入关键帧，加入全局map

5 进行数据融合。在每组关键帧（当前情况下没有新的关键帧加入为一组关键帧）处理完成之后，对当前关键帧（每组的最后一关键帧）进行关键帧的融合（融合的是当前关键帧及其共视关键帧），对地图点进行融合（融合的是当前关键帧的地图点和两级相邻关键帧（关键帧的共视关键帧和所有共视关键帧的共视关键帧）的地图点）。

6 将关键帧交给回环检测

7设置当前可以接受新的关键帧到系统中，即在局部建图（处理关键帧）过程中允许新关键帧集有变动

局部建图线程实际做的就是维护一个好的map，处理tracking线程下来的关键帧。

既然是维护一个好的map，那么一个map中包含了哪些元素，我们需要维护哪些元素呢？

维护全局map主要做的工作是插入关键帧和地图点，删除（去除）冗余关键帧和地图点。

1. 关于数据（插入关键帧和地图点）插入全局map的说明：

插入地图点和关键帧操作：建立地图点并设置地图点属性（a设置观察到该地图点的关键帧，b计算地图点的最优描述子，c计算地图点的平均观测方向和深度范围），建立关键帧并设置地图点关键帧属性（a设置该关键帧中的地图点 b.计算关键帧的BOW向量）。其中将关键帧加入地图map是在Tracking::CreateInitialMapMonocular()（单目初始化帧），Tracking::StereoInitialization()（双目及rgbd相机初始化帧），LocalMapping::ProcessNewKeyFrame()（其他关键帧插入）；地图点插入全局地图map是在Tracking::CreateInitialMapMonocular()（单目初始化帧），Tracking::StereoInitialization()（双目及rgbd相机初始化帧），Tracking::CreateNewKeyFrame()和LocalMapping::CreateNewMapPoints() (非单目其他关键帧地图点插入)，LocalMapping::CreateNewMapPoints()（单目其他关键帧地图点）。非单目地图点的插入可以直接在tracking中进行，因为他们可以直接得到深度信息，从而直接得到地图点的三维点坐标，而单目地图点求其深度信息，需要对极约束和三角化，而对于地图点的三角化和对极约束我们在localmapping线程LocalMapping::CreateNewMapPoints()函数中进行的。

2. 关于数据（去除冗余关键帧和冗余地图点）融合的说明：

冗余关键帧的删除是在localmapping线程中，当一组关键帧运行完成之后（当前新关键帧集中没有新的关键帧）进行关键帧的融合，融合的是当前关键帧（当前组关键帧的最后一帧）及其共视关键帧（有相同地图点的关键帧）。融合的判定条件是：当前关键帧中有90%以上的地图点在其他关键帧中能够找到，则认为该关键帧是冗余的。去除冗余关键帧的方法时将该帧设为bad帧，KeyFrame::SetBadFlag()函数，做如下操作：（1）验证该帧是否可以被擦除（2）擦除所有本关键帧与关联关键帧之间的联系（3）擦除所有地图点与本关键帧之间的关联，标志本关键帧已经不能看到这些地图点,这些地图点也不会存在这些关键帧 （4）清空存储与本关键帧关联的其他关键帧变量，清空排序之后的关联关键帧序列（5）清空子关键帧 并找每个子关键帧的新的父关键帧（6）在地图点和关键帧数据集中剔除本关键帧

冗余地图点的删除是在localmapping线程中SearchInNeighbors()函数下进行的，对当前关键帧中的所有地图点与所有一级二级相邻关键帧（一级关键帧是指当前关键帧的共视关键帧，二级关键帧是指当前关键帧共视关键帧的共视关键帧）中的地图点进行检测融合，通过函数ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)将地图点投影到关键帧的方式进行特征匹配选取最优的匹配点，如果匹配点与当前点的描述子距离小于阈值的情况下进行融合。融合的方式是用被观察次数大的地图点代替被观察次数少的地图点，用函数MapPoint::Replace(MapPoint* pMP)融合，融合地图点作如下操作：（1）将被替换地图点的被观察次数,被查找次数,以及观察到该地图点的关键帧都清空,坏地图点标志置位（2）将被替换地图点的被观察次数,被查找次数都加到替换地图点pMP中,并将当前地图点在关键帧中的位置用代替地图点代替（3）最后将本地图点从全局地图map中删除。

该线程中涉及到的算法有：在建立新关键点时涉及到的对极约束的知识；局部BA优化的知识。下面分别介绍

对极约束与三角测量

为了得到地图中的地图点深度信息，我们在单目情形下需要对匹配点进行对极约束和三角测量。

对于初始化帧的对极约束在Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)函数中实现，对于其他单目帧的对极约束和三角测量是在LocalMapping::CreateNewMapPoints()中实现。

理论如下：

对极几何：

 

如图所示为对极几何示意图，假设有空间点,在帧图像上的投影为，在第二帧图像上的投影为，第一帧相机中心为C，第二帧相机中心为C’，如何通过第一帧的像素坐标来计算第二帧的像素坐标呢？通过图我们可以看出，根据两相机中心坐标和x点可以确定一个平面。这个平面与第二帧像素平面的交线为，可以确定的是，肯定在这一直线上，直线我们称为极线，平面称为极平面，如（b）中所示e和e’称为极点。

在ORBSLAM中我们为了判断两匹配点是否满足对极约束，我们将第一幅图像的中的像素点根据对极几何的方式映射到第二幅图像中，然后计算第二幅图像中的匹配点到映射极线的距离作为判断当前匹配点的效果好坏。通常情况下我们用基础矩阵来描述两匹配像素点的转换关系，所以有，计算到的距离就可以评测匹配点的好坏了。这一部分实现代码ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)

三角测量：

三角测量的用途是用来确定深度信息的，从而确定地图点的三维点坐标。

已知匹配特征点对{}和各自相机矩阵{},估计三维点X

则存在：，他们都属于,即：

采用DLT的方法，x叉乘PX=0，得到：

以上是针对x，对于x’来讲同样存在上述关系，因此存在：

     ->AX=0

将上式写成A=，通过SVD分解的方式得到X的解，从而确定匹配点的三维点坐标。

SVD的分解求解方法参考另一篇文章（ORBSLAM中的单目初始化处理方式）。

局部BA优化

（这一部分我们在另一篇文章（ORBSLAM中的优化问题）中讲解）