orbSLAM2之 局部建图(LocalMapping)线程

概述

该线程的入口是LocalMapping::Run()函数，执行其无限while循环。
下面介绍while循环体一次循环执行的操作。
1，设置LocalMaping类的成员mbAcceptKeyFrames为false，表示不接受Tracking线程创建新关键帧
2，检查成员mlNewKeyFrames中是否有新关键帧。mlNewKeyFrames保存所有Tracking线程创建新关键帧。
3，如果mlNewKeyFrames为空，则检查是否有外部操作要求终止该线程，有的话终止，没有的话休眠3毫秒后进行下一次循环。
4，如果mlNewKeyFrames不为空，则在本次循环中处理一帧新关键帧并进行一系列相关操作，再执行3，退出或进入下一次循环。
如何处理mlNewKeyFrames中的一帧新关键帧是关键，下面详细介绍。

新关键帧处理及相关操作

1，处理新关键帧ProcessNewKeyFrame();

1，将mlNewKeyFrames中的首元素赋值给mpCurrentKeyFrame。mpCurrentKeyFrame即是我们接下来要处理的新关键帧。

2，计算mpCurrentKeyFrame的BoWVector和FeatureVector

3，在生成mpCurrentKeyFrame的普通帧mpCurrentFrame追踪的那些地图点中添加对mpCurrentKeyFrame的记录。在tracking线程中，我们只在mpCurrentFrame中添加了其匹配到的地图点的记录，没有在这些地图点中添加对mpCurrentFrame的记录，因为地图点只保存对关键帧的记录。现在我们要在这些地图点中添加对mpCurrentKeyFrame的记录

4，将mpCurrentKeyFrame新创建的地图点添加到mlpRecentAddedMapPoints。在Tracking线程生成关键帧时，我们会将该关键帧【未匹配的特征点】或【已匹配了地图点，但是匹配的地图点的nObs<1的特征点】生成新的地图点（若为单目相机或为双目/RGBD的单目特征点此时生成的地图点的3d坐标为空）。这些新地图点在生成时添加了对mpCurrentKeyFrame的记录。mlpRecentAddedMapPoints是LocalMapping保存新生成的地图点的数据成员，通过对此成员的操作，确定哪些地图点能够最终保留下来

5，根据mpCurrentKeyFrame->mvpMapPoints更新mpCurrentKeyFrame的mConnectedKeyFrameWeights、mvpOrderedConnectedKeyFrames、mvOrderedWeights、mpParent等参数

6，将关键帧添加到地图(Map)中

2，地图点剪裁MapPointCulling();

1，测试mlpRecentAddedMapPoints中的地图点
2，对于不能通过测试的地图点，从mlpRecentAddedMapPoints删除，设置本地图点mbBad=true，并在所有特征帧以及地图中删除对本地图点的记录，但不删除本地图点。
3，遍历mlpRecentAddedMapPoints中的地图点，若某个地图点满足下面三个条件之一，则未能通过测试。

1，地图点mbBad=true 
2，地图点的 mnFound/mnVisible < 0.25，(即追踪局部地图的 优化内点次数/投影成功次数    <0.25)    
 3，在地图点创建后的第2、3个关键帧要被处理时，地图点的nObs<=设定值cnThObs

若某个地图点在不满足上面三个条件，且 满足：

地图点创建后的第3个关键帧要被处理时，地图点还在mlpRecentAddedMapPoints中

则将该地图点从mlpRecentAddedMapPoints删除，地图点通过测试

3，创建新地图点CreateNewMapPoints();

将mpCurrentKeyFrame中未匹配的地图点与其前nn(单目nn=20，双目RGBDnn=10)共视关键帧中还未匹配地图点进行匹配(BoW加速)、三角化，进而创建新地图点.
1，将mpCurrentKeyFrame的前nn个共视权重最大的共视关键帧保存到vpNeighKFs

2，如果mlNewKeyFrames中还有待处理关键帧，那么只用与mpCurrentKeyFrame共视最多的关键帧pKF2生成新地图点。反之，用所有vpNeighKFs中的共视关键帧pKF2生成新地图点，依次来提高处理速度。

3，通过光心距离判断pKF2是否可用来与mpCurrentKeyFrame三角化。
计算pKF2和mpCurrentKeyFrame的光心距离
如果是双目/RGBD，只有当光心间距大于双目/RGBD基线时，pKF2可用
如果是单目，只有当 光心间距/[共视帧帧观测到的地图点的深度值中位数] >= 0.01时，pKF2可用

4，计算mpCurrentKeyFrame和pKF2之间的基础矩阵F12

5，通过 matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false)匹配mpCurrentKeyFrame和*pKF2中还未匹配地图点的特征点，保存到vMatchedIndices。
关于函数SearchForTriangulation：

//匹配pKF1和*pKF2中还未匹配地图点的特征点，保存到vMatchedPairs(<在pKF1的特征点的索引,匹配的pKF2中的点的索引>),返回匹配数
//如果bOnlyStereo=true(缺省为false)，则只对Stereo特征点尝试匹配
//通过BoW加速匹配：只对有相同NodeId的特征点尝试匹配
//匹配过程：对pKF1中的每一个特征idx1，与pKF2的同一NodeId的每一个特征计算描述子距离，选择其中最距离最小的特征bestIdx2，然后进行一下检查：
//          1，如果idx1和bestIdx2都是单目特征点，要检查角:光心1-地图点-光心2是否满足，以控制三角化误差 
//          2，通过基础矩阵判断两个特征点是否满足对极约束
//          3，如果mbCheckOrientation=true还要近性方向一致性检查(mbCheckOrientation在此类构造时初始化)
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)
~~~s

6，根据不同情况分别采用以下3种方法之一进行计算vMatchedIndices中匹配特征对对应的3d坐标。
~~~s
1，用三角化计算新地图点的3d坐标
2，用mpCurrentKeyFrame中的3d坐标作为新地图点的3d坐标
3，用pKF2中的3d坐标作为新地图点的3d坐标

7，通过以下条件检查上一步中计算的3d坐标是否满足精度

1，检查在两帧中深度是否为正
2，检查在两帧中的重投影误差是否
		<=5.991*sigmaSquare2（对于单目特征点）
		<=7.8*sigmaSquare2（对于双目特征点）
		sigmaSquare2是特征点在该帧所在的金字塔尺度的平方
3，检查尺度(比)的一致性
		即地图点到两光心的距离之比和特征点在两帧上的所在的金字塔尺度之比是否接近

8，为通过第7步检查的3d坐标，生成地图点。
在mpCurrentKeyFrame和pKF2中添加对新地图点的观测
在新地图点中添加对mpCurrentKeyFrame和pKF2的记录
计算新地图点的描述子，更新其mfMaxDistance, mfMinDistance, mNormalVector等参数
将地图点添加到Map和LocalMap的mlpRecentAddedMapPoints中

4，地图点融合SearchInNeighbors();

若mlNewKeyFrames为空才进行地图点融合，以此减少计算量。

SearchInNeighbors()函数：匹配 mpCurrentKeyFrame和[共视帧以及共视帧的共视帧]之间的地图点和特征，融合有关地图点(不生成新地图点)，更新mpCurrentKeyFrame的共视信息

5，LocalBA

LocalBA不是必然会进行的。只有当mlNewKeyFrames为空且没有外部操作终止本线程，并且地图中的关键帧数>2时才进行。
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

//参与优化的帧(非Fixed)：pKF及其权重>15的关键帧
//参与优化的地图点：非Fixed帧匹配的所有地图点
//参与优化的帧(Fixed)：参与优化的地图点的所有观测帧(除了非Fixed的帧)
//初步优化，然后根据pbStopFlag判断是否进行二次优化
//二次优化，设置初步优化后残差过大或地图点深度为负的边不再优化，取消所有边的核函数的边不再优化，进行二次优化
//将二次优化后残差过大或地图点深度为负的边涉及的关键帧和地图点，取消对彼此的记录
//保存优化结果，对优化涉及的所有关键帧和地图点都更新为优化结果
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)

6，关键帧剪裁 KeyFrameCulling();

//冗余的关键帧的删除
//待检查关键帧：mpCurrentKeyFrame的共视关键帧(权重>15)
//对待检查关键帧中的每一帧pKF检查是否满足：nRedundantObservations>0.9*nMPs，若满足则该帧冗余。
//      nRedundantObservations —— 冗余观测数：对于双目/RGBD相机，是[在pKF中为近stereo地图点]且[在任意其他3个关键帧中有尺度满足(质量好)的观测]的地图点数
//                                              对于单目相机，是在任意其他3个关键帧中有尺度满足(质量好)的观测]的地图点数
//      nMPs                   —— 观测数：     对于双目/RGBD相机，是[在pKF中为近stereo地图点]
//                                               对于单目相机，是观测地图点数
/对于冗余的关键帧，设置其为坏帧(mbBad = true)，删除其他帧、地图点、地图对于该帧的记录，但不删除该帧

KeyFrameCulling();

7，向闭环检测线程添加一个新的关键帧

mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);