ORB-SLAM2代码整理--LocalMapping线程
Tracking线程 只是判断当前帧是否需要加入关键帧，并没有真的加入地图， 因为Tracking线程的主要功能是局部定位， 而处理地图中的关键帧，地图点，包括如何加入，如何删除的工作是在LocalMapping线程完成的
LocalMapping::Run()1 SetAcceptKeyFrames(false);
告诉Tracking，LocalMapping正处于繁忙状态，
// LocalMapping线程处理的关键帧都是Tracking线程发过的
// 在LocalMapping线程还没有处理完关键帧之前Tracking线程最好不要发送太快2 CheckNewKeyFrames()
检查队列,等待处理的关键帧列表不为空.
3 ProcessNewKeyFrame()
计算关键帧特征点的词典单词向量BoW映射，将关键帧插入地图
/**
* @brief 处理列表中的关键帧
* - 计算Bow，加速三角化新的MapPoints
* - 关联当前关键帧至MapPoints，并更新MapPoints的平均观测方向和观测距离范围
* - 插入关键帧，更新Covisibility图和Essential图
* @see VI-A keyframe insertion
*/3.1 从缓冲队列中取出一帧关键帧
3.2 计算该关键帧特征点的Bow映射关系
3.3 跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定
// 在TrackLocalMap函数中将局部地图中的MapPoints与当前帧进行了匹配，
// 但没有对这些匹配上的MapPoints与当前帧进行关联3.4 非当前帧生成的MapPoints,为当前帧在tracking过程跟踪到的MapPoints更新属性
获得该点的平均观测方向和观测距离范围
加入关键帧后，更新3d点的最佳描述子
3.5 当前帧生成的MapPoints
// 将双目或RGBD跟踪过程中新插入的MapPoints放入mlpRecentAddedMapPoints，等待检查
// CreateNewMapPoints函数中通过三角化也会生成MapPoints
// 这些MapPoints都会经过MapPointCulling函数的检验3.6 UpdateConnections更新关键帧间的连接关系，Covisibility图和Essential图(tree
/**
* @brief 更新图的连接
*
* 1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键与其它所有关键帧之间的共视程度
* 对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
* 2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较 高）
* 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理
* 更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树
*/
void KeyFrame::UpdateConnections()
{
// 在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系
//===============1==================================
map KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数
vector vpMP;
{
// 获得该关键帧的所有3D点
unique_lock lockMPs(mMutexFeatures);
vpMP = mvpMapPoints;
}
//For all map points in keyframe check in which other keyframes are they seen
//Increase counter for those keyframes
// 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
// 即统计每一个关键帧都有多少关键帧与它存在共视关系，统计结果放在KFcounter
for(vector::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(!pMP)
continue;
if(pMP->isBad())
continue;
// 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧
map observations = pMP->GetObservations();
for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
// 除去自身，自己与自己不算共视
if(mit->first->mnId==mnId)
continue;
KFcounter[mit->first]++;
}
}
// This should not happen
if(KFcounter.empty())
return;
//===============2==================================
// If the counter is greater than threshold add connection
// In case no keyframe counter is over threshold add the one with maximum counter
int nmax=0;
KeyFrame* pKFmax=NULL;
int th = 15;
// vPairs记录与其它关键帧共视帧数大于th的关键帧
// pair将关键帧的权重写在前面，关键帧写在后面方便后面排序
vector > vPairs;
vPairs.reserve(KFcounter.size());
for(map::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
{
if(mit->second>nmax)
{
nmax=mit->second;
// 找到对应权重最大的关键帧（共视程度最高的关键帧）
pKFmax=mit->first;
}
if(mit->second>=th)
{
// 对应权重需要大于阈值，对这些关键帧建立连接
vPairs.push_back(make_pair(mit->second,mit->first));
// 更新KFcounter中该关键帧的mConnectedKeyFrameWeights(没有链接的链接,链接不是对应权重,链接对应权重)+UpdateBestCovisibles()(按照权重对连接的关键帧进行排序 更新后的变量存储在mvpOrderedConnectedKeyFrames和mvOrderedWeights中);
// 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
(mit->first)->AddConnection(this,mit->second);
}
}
// 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
if(vPairs.empty())
{
// 如果每个关键帧与它共视的关键帧的个数都少于th，
// 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
// 这是对之前th这个阈值可能过高的一个补丁
vPairs.push_back(make_pair(nmax,pKFmax));
pKFmax->AddConnection(this,nmax);
}
// vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由大到小
sort(vPairs.begin(),vPairs.end());
list lKFs;
list lWs;
for(size_t i=0; i lockCon(mMutexConnections);
// mspConnectedKeyFrames = spConnectedKeyFrames;
// 更新图的连接(权重)
mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重
mvpOrderedConnectedKeyFrames = vector(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector(lWs.begin(), lWs.end());
// 更新生成树的连接
if(mbFirstConnection && mnId!=0)
{
// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
mpParent = mvpOrderedConnectedKeyFrames.front();
// 建立双向连接关系
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
}
3.7 将该关键帧插入到地图中
4 MapPointCulling()
剔除ProcessNewKeyFrame函数中引入的不合格MapPointsvoid LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints
list::iterator lit = mlpRecentAddedMapPoints.begin();
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
int nThObs;
if(mbMonocular)
nThObs = 2;
else
nThObs = 3;
const int cnThObs = nThObs;
// 遍历等待检查的MapPoints
while(lit!=mlpRecentAddedMapPoints.end())
{
MapPoint* pMP = *lit;
if(pMP->isBad())
{
// 步骤1：已经是坏点的MapPoints直接从检查链表中删除
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(pMP->GetFoundRatio()<0.25f)
{
// 步骤2：将不满足VI-B条件的MapPoint剔除
// VI-B 条件1：
// 跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例需大于25%
// IncreaseFound / IncreaseVisible < 25%，注意不一定是关键帧。
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
{
// 步骤3：将不满足VI-B条件的MapPoint剔除
// VI-B 条件2：从该点建立开始，到现在已经过了不小于2个关键帧
// 但是观测到该点的关键帧数却不超过cnThObs帧，那么该点检验不合格
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
// 步骤4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
// 因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测
lit = mlpRecentAddedMapPoints.erase(lit);
else
lit++;
}
}
5 CreateNewMapPoints()
相机运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints5.1 在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
5.2 遍历相邻关键帧vpNeighKFs
得到邻接关键帧的Ow2
进而得到两个关键帧之间的相机位移vBaseline = Ow2-Ow1;与基线长度baseline = cv::norm(vBaseline);
5.3 判断相机运动的基线是否够长
邻接关键帧的场景深度中值medianDepthKF2
baseline与景深的比例(若特别小,即两个关键帧特别远,不考虑此邻接关键帧,不生成3D点
5.4 根据两个关键帧的位姿计算它们之间的基本矩阵
5.5 通过极线约束限制匹配时的搜索范围，进行特征点匹配
SearchForTriangulation通过基础矩阵找到匹配的特征点
具体的看一下代码,都是一些细节.这里记录一下基础矩阵与极线的关系:
可以表示二位平面的直线方程,若点为归一化的点则直线l可以表示为
基础矩阵F满足:
具体推导见高博十四讲P142
对于右边的图像平面来说,p2点在极线l上,故
,所以有l2=Fp1
极点e2在l2上,故
,e过所有的极线,故p1可以为任意值,故有
以上也就是CheckDistEpipolarLine函数所用到的,计算p2点到极线的距离
5.6 对每对匹配通过三角化生成3D点,和 Triangulate函数差不多
取出匹配特征点即当前匹配对在当前关键帧中的索引和在邻接关键帧的索引,根据索引取出在当前关键帧和邻接关键帧中的特征点和其深度
利用匹配点反投影得到视差角
根据两个向量求余弦值:
得到双目观测的视差角
三角化恢复3D点
cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常
cosParallaxRays 视差角度小时用三角法恢复3D点，视差角大时用双目恢复3D点（双目以及深度有效）(通过三角法的那个叉乘待仔细解决) 检测生成的3D点是否在相机前方 计算3D点在当前关键帧下的重投影误差 基于卡方检验计算出的阈值（假设测量有一个像素的偏差） 计算3D点在另一个关键帧下的重投影误差 基于卡方检验计算出的阈值（假设测量有一个一个像素的偏差） 5.7 检查尺度的连续性 ratioDist*ratioFactor < ratioOctave 或 ratioDist/ratioOctave > ratioFactor表明尺度变化是连续的 5.8 三角化生成3D点成功，构造成MapPoint,并为mappoint添加属性 5.9 将新产生的点放入检测队列mlpRecentAddedMapPoints中 6 if(!CheckNewKeyFrames())再次检查, 其实是一个循环,直到处理完队列中的最后一帧 7 SearchInNeighbors 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints 7.1 获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧 找到当前帧一级相邻与二级相邻关键帧 7.2 将当前帧的MapPoints分别与一级二级相邻帧(的MapPoints)进行融合 vpFuseCandidates来存储一级邻接和二级邻接关键帧所有MapPoints的集合 7.3 将当前帧的一级二级邻接关键帧的mappoints分别与当前帧(的mappoints)进行融合 7.4 更新当前帧MapPoints的描述子(最好的描述子与其他描述子应该具有最小的距离中值)，深度，观测主方向等属性 7.5 更新当前帧的MapPoints后更新与其它帧的连接关系(更新covisibility图的连接) 见3.6 8 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping 9 进行一个LocalBundleAdjustment 这个局部BA不同于tracking线程的局部BA.这个优化函数里面顶点类型有两个,所以对pose和顶点位置都进行了优化 9.1 将当前关键帧加入lLocalKeyFrames 9.2 找到关键帧连接的关键帧（一级相连），加入lLocalKeyFrames中 9.3 遍历lLocalKeyFrames中关键帧，将它们观测的MapPoints加入到lLocalMapPoints 9.4 得到能被局部MapPoints观测到，但不属于局部关键帧的关键帧，这些关键帧在局部BA优化时不优化 上图选自:路游侠的博客 其中pose1应该就是所说的不属于局部关键帧的关键帧 9.5 构造g2o优化器 9.6 添加顶点：Pose of Local KeyFrame 9.7 添加顶点：Pose of Fixed KeyFrame，注意这里调用了vSE3->setFixed(true)。 9.8 添加顶点:mappoint 9.9 构建边：对每一对关联的MapPoint和KeyFrame 9.10 开始优化 9.11 检测outlier，并设置下次不优化 9.12 排除误差较大的outlier后再次优化 9.13 优化后再次计算误差, 9.14 优化后更新关键帧位姿以及MapPoints的位置、平均观测方向等属性 10 KeyFrameCulling() 11 将当前关键帧加入到闭环检测队列中 // 这些MapPoints都会经过MapPointCulling函数的检验
//将当前帧的所有mappoints全取出,寻找重复的 // 投影当前帧的MapPoints到相邻关键帧pKFi中，并判断是否有重复的MapPoints
// 1.如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
// 2.如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint
vSE3
vPoint
检测并剔除当前帧相邻的关键帧中冗余的关键帧
// 剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到
// trick!
// Tracking中先把关键帧交给LocalMapping线程
// 并且在Tracking中InsertKeyFrame函数的条件比较松，交给LocalMapping线程的关键帧会比较密
mlpLoopKeyFrameQueue