ORB SLAM2是在ORB SLAM基础上提出的一种性能十分优异的SLAM算法，主要通过Tracking（跟踪）、Local Mapping（本地制图）、Loop Closing（闭环检测）三个并行线程，流程如下图所示。本篇博客主要针对LocalMapping线程展开。
注：Tracking部分的代码可以参照博主之前的博客：ORB SLAM2 Tracking部分论文及代码解读
代码解读
这里我们直接开始看代码，通过解析代码来更明确详细的阐述ORB SLAM2 Local Mapping部分的内容。
以单目为例。首先我们来看一下Local Mapping的构造函数：
// 构造函数
LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
{
/*
* mbStopRequested： 外部线程调用，为true，表示外部线程请求停止 local mapping
* mbStopped： 为true表示可以并终止localmapping 线程
* mbNotStop： true，表示不要停止 localmapping 线程，因为要插入关键帧了。需要和 mbStopped 结合使用
* mbAcceptKeyFrames： true，允许接受关键帧。tracking 和local mapping 之间的关键帧调度
* mbAbortBA： 是否流产BA优化的标志位
* mbFinishRequested： 请求终止当前线程的标志。注意只是请求，不一定终止。终止要看 mbFinished
* mbResetRequested： 请求当前线程复位的标志。true，表示一直请求复位，但复位还未完成；表示复位完成为false
* mbFinished： 判断最终LocalMapping::Run() 是否完成的标志。
*/
}
ORB SLAM2中Local Mapping部分主要依托LocalMapping::Run()函数完成，LocalMapping::Run()函数在LocalMapping.cc文件中，下面进入到这个函数中做进一步说明：
void LocalMapping::Run()
{
// 标记状态，表示当前run函数正在运行，尚未结束
mbFinished = false;
// 主循环
while(1)
{
// Tracking will see that Local Mapping is busy
// Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我
// LocalMapping线程处理的关键帧都是Tracking线程发过的
SetAcceptKeyFrames(false);
首先将mbFinished置为false，表示LocalMapping的线程在正运行，然后进入到while(1)的循环中，然后跳入到SetAcceptKeyFrames()函数中，SetAcceptKeyFrames()函数在LocalMapping.cc文件中，具体如下
// 设置"允许接受关键帧"的状态标志
void LocalMapping::SetAcceptKeyFrames(bool flag)
{
unique_lock<mutex> lock(mMutexAccept);
mbAcceptKeyFrames=flag;
}
}
由于SetAcceptKeyFrames()函数传入的参数为false，因此将mbAcceptKeyFrames置为flase，即不允许接收新的关键帧（来自Tracking线程的关键帧）
然后回到LocalMapping::Run()函数中，由于下面的分支函数较多，这里一次性展示一个完整的if语句，然后分别进行说明。
// Check if there are keyframes in the queue
// 等待处理的关键帧列表不为空
if(CheckNewKeyFrames())
{
// BoW conversion and insertion in Map
// Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
ProcessNewKeyFrame();
// Check recent MapPoints
// Step 3 根据地图点的观测情况剔除质量不好的地图点
MapPointCulling();
// Triangulate new MapPoints
// Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
CreateNewMapPoints();
// 已经处理完队列中的最后的一个关键帧
if(!CheckNewKeyFrames())
{
// Find more matches in neighbor keyframes and fuse point duplications
// Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
SearchInNeighbors();
}
// 终止BA的标志
mbAbortBA = false;
// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
if(!CheckNewKeyFrames() && !stopRequested())
{
// Local BA
// Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BA
if(mpMap->KeyFramesInMap()>2)
// 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// Check redundant local Keyframes
// Step 7 检测并剔除当前帧相邻的关键帧中冗余的关键帧
// 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到
KeyFrameCulling();
}
// Step 8 将当前帧加入到闭环检测队列中
// 注意这里的关键帧被设置成为了bad的情况,这个需要注意
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
CheckNewKeyFrames()函数在LocalMapping.cc文件中，用来判断等待处理的关键帧列表是否为空，如果非空，则进入if语句中，执行ProcessNewKeyFrame()函数：
void LocalMapping::ProcessNewKeyFrame()
{
void LocalMapping::ProcessNewKeyFrame()
{
// Step 1：从缓冲队列中取出一帧关键帧
// 该关键帧队列是Tracking线程向LocalMapping中插入的关键帧组成
{
unique_lock<mutex> lock(mMutexNewKFs);
// 取出列表中最前面的关键帧，作为当前要处理的关键帧
mpCurrentKeyFrame = mlNewKeyFrames.front();
// 取出最前面的关键帧后，在原来的列表里删掉该关键帧
mlNewKeyFrames.pop_front();
}
// Compute Bags of Words structures
// Step 2：计算该关键帧特征点的Bow信息
mpCurrentKeyFrame->ComputeBoW();
}
// Associate MapPoints to the new keyframe and update normal and descriptor
// Step 3：当前处理关键帧中有效的地图点，更新normal，描述子等信息
// TrackLocalMap中和当前帧新匹配上的地图点和当前关键帧进行关联绑定
const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
// 对当前处理的这个关键帧中的所有的地图点展开遍历
for(size_t i=0; i<vpMapPointMatches.size(); i++)
{
MapPoint* pMP = vpMapPointMatches[i];
if(pMP)
{
if(!pMP->isBad())
{
if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
{
// 如果地图点不是来自当前帧的观测，为当前地图点添加观测
pMP->AddObservation(mpCurrentKeyFrame, i);
// 获得该点的平均观测方向和观测距离范围
pMP->UpdateNormalAndDepth();
// 更新地图点的最佳描述子
pMP->ComputeDistinctiveDescriptors();
}
else // this can only happen for new stereo points inserted by the Tracking
{
// 如果当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息
// 这些地图点可能来自双目或RGBD跟踪过程中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生
// 将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验
mlpRecentAddedMapPoints.push_back(pMP);
}
}
}
}
首先取出列表中最前面的关键帧，作为当前要处理的关键帧，并将其从原始列表中删除，然后使用ComputeBoW()函数来计算关键帧特征点的BoW信息，使用GetMapPointMatches()函数获取当前帧具体的地图点，然后使用一个for循环对当前帧对应的地图点进行遍历，用pMP指针取出每一个地图点，首先判断地图点是否存在，确认存在后判断地图点是否为坏点，进而判断该地图点是否在当前关键帧中（pMP->IsInKeyFrame(mpCurrentKeyFrame)部分），如果该地图点不是来自当前帧的观测，则为当前地图点添加观测，并计算该地图点的平均观测方向和观测距离，更新地图点的最佳描述子。如果当前帧中已经包含了该地图点，由于这些当前帧是新生成的关键帧，所以需要向地图点中添加新生成关键帧的信息，但是由于这些地图点可能来自双目或RGBD跟踪过程中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生，所以将上述的地图点加入到mlpRecentAddedMapPoints，等待后续MapPointCulling()函数的检验。其中，ComputeDistinctiveDescriptors()这个函数比较重要，我们进入其中：
void MapPoint::ComputeDistinctiveDescriptors()
{
// Retrieve all observed descriptors
vector<cv::Mat> vDescriptors;
map<KeyFrame*,size_t> observations;
// Step 1 获取所有观测，跳过坏点
{
unique_lock<mutex> lock1(mMutexFeatures);
if(mbBad)
return;
observations=mObservations;
}
if(observations.empty())
return;
vDescriptors.reserve(observations.size());
// Step 2 遍历观测到3d点的所有关键帧，获得orb描述子，并插入到vDescriptors中
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
// mit->first取观测到该地图点的关键帧
// mit->second取该地图点在关键帧中的索引
KeyFrame* pKF = mit->first;
if(!pKF->isBad())
vDescriptors.push_back(pKF->mDescriptors.row(mit->second)); // 取对应的描述子向量
}
if(vDescriptors.empty())
return;
// Compute distances between them
// Step 3 获得这些描述子两两之间的距离
// N表示为一共多少个描述子
const size_t N = vDescriptors.size();
// 将Distances表述成一个对称的矩阵
// float Distances[N][N];
std::vector<std::vector<float> > Distances;
Distances.resize(N, vector<float>(N, 0));
for (size_t i = 0; i<N; i++)
{
//和自己的距离当然是0
Distances[i][i]=0;
for(size_t j=i+1;j<N;j++)
{
int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
Distances[i][j]=distij;
Distances[j][i]=distij;
}
}
// Take the descriptor with least median distance to the rest
// Step 4 选择最有代表性的描述子，它与其他描述子应该具有最小的距离中值
int BestMedian = INT_MAX;
int BestIdx = 0;
for(size_t i=0;i<N;i++)
{
// 第i个描述子到其它所有所有描述子之间的距离
// vector<int> vDists(Distances[i],Distances[i]+N);
vector<int> vDists(Distances[i].begin(), Distances[i].end());
sort(vDists.begin(), vDists.end());
// 获得中值
int median = vDists[0.5*(N-1)];
// 寻找最小的中值
if(median<BestMedian)
{
BestMedian = median;
BestIdx = i;
}
}
{
unique_lock<mutex> lock(mMutexFeatures);
mDescriptor = vDescriptors[BestIdx].clone();
}
}
ComputeDistinctiveDescriptors()函数在MapPoint.cc文件中，由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子，主要思路是先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。
回到LocalMapping::ProcessNewKeyFrame()函数中，
// Update links in the Covisibility Graph
// Step 4：更新关键帧间的连接关系（共视图）
mpCurrentKeyFrame->UpdateConnections();
// Insert Keyframe in Map
// Step 5：将该关键帧插入到地图中
mpMap->AddKeyFrame(mpCurrentKeyFrame);
}
接着更新关键帧的连接关系，也就是共视图，并将关键帧插入到地图中，然后进入到UpdateConnections()函数中
void KeyFrame::UpdateConnections()
{
// 在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系
map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数
vector<MapPoint*> vpMP;
{
// 获得该关键帧的所有3D点
unique_lock<mutex> 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点的所有关键帧之间的共视程度
// Step 1 统计每一个地图点都有多少关键帧与当前关键帧存在共视关系，统计结果放在KFcounter
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(!pMP)
continue;
if(pMP->isBad())
continue;
// 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧
map<KeyFrame*,size_t> observations = pMP->GetObservations();
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
// 除去自身，自己与自己不算共视
if(mit->first->mnId==mnId)
continue;
// 这里的操作非常精彩！
// map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
// mit->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
// 所以最后KFcounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧的地图点，也就是共视程度
KFcounter[mit->first]++;
}
}
// This should not happen
if(KFcounter.empty())
return;
// 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;
// 至少有15个共视地图点
int th = 15;
// vPairs记录与其它关键帧共视帧数大于th的关键帧
// pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
vector<pair<int,KeyFrame*> > vPairs;
vPairs.reserve(KFcounter.size());
// Step 2 找到对应权重最大的关键帧（共视程度最高的关键帧）
for(map<KeyFrame*,int>::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
// 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
(mit->first)->AddConnection(this,mit->second);
}
}
// Step 3 如果没有连接到关键（超过阈值的权重），则对权重最大的关键帧建立连接
if(vPairs.empty())
{
// 如果每个关键帧与它共视的关键帧的个数都少于th，
// 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
// 这是对之前th这个阈值可能过高的一个补丁
vPairs.push_back(make_pair(nmax,pKFmax));
pKFmax->AddConnection(this,nmax);
}
// Step 4 对共视程度比较高的关键帧对更新连接关系及权重（从大到小）
// vPairs里存的都是相互共视程度比较高的关键帧和共视权重，接下来由大到小进行排序
sort(vPairs.begin(),vPairs.end()); // sort函数默认升序排列
// 将排序后的结果分别组织成为两种数据类型
list<KeyFrame*> lKFs;
list<int> lWs;
for(size_t i=0; i<vPairs.size();i++)
{
// push_front 后变成了从大到小顺序
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
}
{
unique_lock<mutex> lockCon(mMutexConnections);
// mspConnectedKeyFrames = spConnectedKeyFrames;
// 更新当前帧与其它关键帧的连接权重
mConnectedKeyFrameWeights = KFcounter;
mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
// Step 5 更新生成树的连接
if(mbFirstConnection && mnId!=0)
{
// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
mpParent = mvpOrderedConnectedKeyFrames.front();
// 建立双向连接关系，将当前关键帧作为其子关键帧
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
}
- 更新图的连接
- 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
- 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
- 对这些连接按照权重从大到小进行排序，以方便将来的处理，更新完共视图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树。
回到LocalMapping::ProcessNewKeyFrame()函数中，mpMap->AddKeyFrame(mpCurrentKeyFrame)函数的主要作用就是在地图中插入关键帧,同时更新关键帧的最大id。至此，LocalMapping::ProcessNewKeyFrame()函数完成，返回到LocalMapping::Run()函数中，接下来进入到MapPointCulling()函数中：
void LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints
list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
// Step 1：根据相机类型设置不同的观测阈值
int nThObs;
if(mbMonocular)
nThObs = 2;
else
nThObs = 3;
const int cnThObs = nThObs;
// Step 2：遍历检查的新添加的MapPoints
while(lit!=mlpRecentAddedMapPoints.end())
{
MapPoint* pMP = *lit;
if(pMP->isBad())
{
// Step 2.1：已经是坏点的MapPoints直接从检查链表中删除
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(pMP->GetFoundRatio()<0.25f)
{
// Step 2.2：跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例小于25%，删除
// (mnFound/mnVisible） < 25%
// mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好
// mnVisible：地图点应该被看到的次数
// (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
{ // Update points
// Step 5：更新当前帧地图点的描述子、深度、观测主方向等属性
vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
{
MapPoint* pMP=vpMapPointMatches[i];
if(pMP)
{
if(!pMP->isBad())
{
// 在所有找到pMP的关键帧中，获得最佳的描述子
pMP->ComputeDistinctiveDescriptors();
// 更新平均观测方向和观测距离
pMP->UpdateNormalAndDepth();
}
}
}
// Update connections in covisibility graph
// Step 6：更新当前帧的MapPoints后更新与其它帧的连接关系
// 更新covisibility图
mpCurrentKeyFrame->UpdateConnections();
// Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧
// 但是观测到该点的关键帧数却不超过cnThObs帧，那么删除该点
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
// Step 2.4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
// 因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测
lit = mlpRecentAddedMapPoints.erase(lit);
else
lit++;
}
}
这个函数的核心是对mlpRecentAddedMapPoints，即通过LocalMapping::ProcessNewKeyFrame()函数新增的地图点（新增关键帧中包含这些地图点的信息，这些地图点缺少相应关键帧中的信息，保存在mlpRecentAddedMapPoints中）做一系列筛选和剔除。首先根据相机类型设置不同的观测阈值，单目为2，双目和RGBD为3。然后进行便利筛选新增的地图点，如果该地图点实际被观测到的帧数和理论上被观测到的帧数的比例小于0.25，则将其置为坏点，并从mlpRecentAddedMapPoints中删除已经筛选完的点；如果如果从该地图点建立到现在，已经经历了不少于两个的关键帧，但是地图点观测到的关键帧数仍然少于观测阈值，则也将其置为坏点，并从mlpRecentAddedMapPoints中删除；从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点，则只是将其从mlpRecentAddedMapPoints中删除。
返回到LocalMapping::Run()函数中，接下来进入到CreateNewMapPoints()函数中:
void LocalMapping::CreateNewMapPoints()
{
// Retrieve neighbor keyframes in covisibility graph
// nn表示搜索最佳共视关键帧的数目
// 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图
int nn = 10;
if(mbMonocular)
nn=20;
// Step 1：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
// 特征点匹配配置 最佳距离 < 0.6*次佳距离，比较苛刻了。不检查旋转
ORBmatcher matcher(0.6,false);
// 取出当前帧从世界坐标系到相机坐标系的变换矩阵
cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
cv::Mat Rwc1 = Rcw1.t(); //R为正交矩阵，矩阵的逆即为其转置
cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
cv::Mat Tcw1(3,4,CV_32F);
Rcw1.copyTo(Tcw1.colRange(0,3));
tcw1.copyTo(Tcw1.col(3));
// 得到当前关键帧（左目）光心在世界坐标系中的坐标、内参
cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
const float &fx1 = mpCurrentKeyFrame->fx;
const float &fy1 = mpCurrentKeyFrame->fy;
const float &cx1 = mpCurrentKeyFrame->cx;
const float &cy1 = mpCurrentKeyFrame->cy;
const float &invfx1 = mpCurrentKeyFrame->invfx;
const float &invfy1 = mpCurrentKeyFrame->invfy;
// 用于后面的点深度的验证;这里的1.5是经验值
// mfScaleFactor = 1.2
const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
// 记录三角化成功的地图点数目
int nnew=0;
设置双目和RGBD的公示关键帧数为10，单目为20，这里仍然以单目为例，用mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn)得到与当前关键帧共视程度最高的20帧相邻关键帧，然后用ORBmatcher matcher(0.6,false)对特征点匹配进行配置，最佳距离 < 0.6*次佳距离，比较苛刻了。不检查旋转。分别取出当前关键帧从世界坐标系到相机坐标系的旋转和平移矩阵，组成变换矩阵Tcw1。然后进一步得到当前关键帧光心在世界坐标系中的坐标以及相机的内参。ratioFactor变量用于点的深度验证，nnew变量用来记录成功三角化的地图点数目。
// Search matches with epipolar restriction and triangulate
// Step 2：遍历相邻关键帧vpNeighKFs
for(size_t i=0; i<vpNeighKFs.size(); i++)
{
// 下面的过程会比较耗费时间,因此如果有新的关键帧需要处理的话,就先去处理新的关键帧吧
if(i>0 && CheckNewKeyFrames())
return;
KeyFrame* pKF2 = vpNeighKFs[i];
// Check first that baseline is not too short
// 邻接的关键帧光心在世界坐标系中的坐标
cv::Mat Ow2 = pKF2->GetCameraCenter();
// 基线向量，两个关键帧间的相机位移
cv::Mat vBaseline = Ow2-Ow1;
// 基线长度
const float baseline = cv::norm(vBaseline);
// Step 3：判断相机运动的基线是不是足够长
if(!mbMonocular)
{
// 如果是双目相机，关键帧间距小于本身的基线时不生成3D点
// 因为太短的基线下能够恢复的地图点不稳定
if(baseline<pKF2->mb)
continue;
}
else
{
// 单目相机情况
// 邻接关键帧的场景深度中值
const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
// baseline与景深的比例
const float ratioBaselineDepth = baseline/medianDepthKF2;
// 如果比例特别小，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点
if(ratioBaselineDepth<0.01)
continue;
}
首先遍历相邻的共视关键帧，如果有新的关键帧需要处理，则结束该函数，如果没有，则计算共视关键帧的光心坐标，然后计算当前关键帧光心坐标和其共视关键帧光心坐标的距离，进一步得到基线长度。再根据相机类型分别判断基线长度是否可行（相机位姿变换太小时，计算出来的变换矩阵误差会很大）。
// Compute Fundamental Matrix
// Step 4：根据两个关键帧的位姿计算它们之间的基础矩阵
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
// Search matches that fullfil epipolar constraint
// Step 5：通过BoW对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对
vector<pair<size_t,size_t> > vMatchedIndices;
matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);
cv::Mat Rcw2 = pKF2->GetRotation();
cv::Mat Rwc2 = Rcw2.t();
cv::Mat tcw2 = pKF2->GetTranslation();
cv::Mat Tcw2(3,4,CV_32F);
Rcw2.copyTo(Tcw2.colRange(0,3));
tcw2.copyTo(Tcw2.col(3));
const float &fx2 = pKF2->fx;
const float &fy2 = pKF2->fy;
const float &cx2 = pKF2->cx;
const float &cy2 = pKF2->cy;
const float &invfx2 = pKF2->invfx;
const float &invfy2 = pKF2->invfy;
通过ComputeF12(mpCurrentKeyFrame,pKF2)计算当前帧到其共视关键帧的位姿变换，然后通过BoW对两关键帧的未匹配的特征点快速匹配(SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false))，用极线约束抑制离群点，生成新的匹配点对。同理，分别取出共视关键帧从世界坐标系到相机坐标系的旋转和平移矩阵，组成变换矩阵Tcw2。然后进一步得到共视关键帧对应的相机的内参。
// Triangulate each match
// Step 6：对每对匹配通过三角化生成3D点,和 Triangulate函数差不多
const int nmatches = vMatchedIndices.size();
for(int ikp=0; ikp<nmatches; ikp++)
{
// Step 6.1：取出匹配特征点
// 当前匹配对在当前关键帧中的索引
const int &idx1 = vMatchedIndices[ikp].first;
// 当前匹配对在邻接关键帧中的索引
const int &idx2 = vMatchedIndices[ikp].second;
// 当前匹配在当前关键帧中的特征点
const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
// mvuRight中存放着双目的深度值，如果不是双目，其值将为-1
const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
bool bStereo1 = kp1_ur>=0;
// 当前匹配在邻接关键帧中的特征点
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
// mvuRight中存放着双目的深度值，如果不是双目，其值将为-1
const float kp2_ur = pKF2->mvuRight[idx2];
bool bStereo2 = kp2_ur>=0;
// Check parallax between rays
// Step 6.2：利用匹配点反投影得到视差角
// 特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量,和这个点的反投影射线重合
cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);
// 由相机坐标系转到世界坐标系(得到的是那条反投影射线的一个同向向量在世界坐标系下的表示,还是只能够表示方向)，得到视差角余弦值
cv::Mat ray1 = Rwc1*xn1;
cv::Mat ray2 = Rwc2*xn2;
// 这个就是求向量之间角度公式
const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
// 加1是为了让cosParallaxStereo随便初始化为一个很大的值
float cosParallaxStereo = cosParallaxRays+1;
float cosParallaxStereo1 = cosParallaxStereo;
float cosParallaxStereo2 = cosParallaxStereo;
// Step 6.3：对于双目，利用双目得到视差角；单目相机没有特殊操作
if(bStereo1)
// 传感器是双目相机,并且当前的关键帧的这个点有对应的深度
// 假设是平行的双目相机，计算出两个相机观察这个点的时候的视差角;
// ? 感觉直接使用向量夹角的方式计算会准确一些啊（双目的时候），那么为什么不直接使用那个呢？
// 回答：因为双目深度值、基线是更可靠的，比特征匹配再三角化出来的稳
cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
else if(bStereo2)
//传感器是双目相机,并且邻接的关键帧的这个点有对应的深度，和上面一样操作
cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
// 得到双目观测的视差角
cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
对每一对匹配点通过三角化生成3D点：首先分别取出每一对匹配点在当前关键帧和共视关键帧中的索引，然后根据索引取出其对应的特征点，进一步将当前关键帧和共视关键帧中的对应特征点投影到各自的相机坐标系下，再投影到世界坐标系下，求解视差角余弦值。
// Step 6.4：三角化恢复3D点关键帧从世界坐标系到相机坐标系的旋转和平移矩阵
cv::Mat x3D;
// cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常,0.9998 对应1°
// cosParallaxRays < cosParallaxStereo 表明视差角很小
// ?视差角度小时用三角法恢复3D点，视差角大时用双目恢复3D点（双目以及深度有效）
// 参考：https://github.com/raulmur/ORB_SLAM2/issues/345
if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
{
// Linear Triangulation Method
// 原理
// Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
// x' = P'X x = PX
// 它们都属于 x = aPX模型
// |X|
// |x| |p1 p2 p3 p4 ||Y| |x| |--p0--||.|
// |y| = a |p5 p6 p7 p8 ||Z| ===>|y| = a|--p1--||X|
// |z| |p9 p10 p11 p12||1| |z| |--p2--||.|
// 采用DLT的方法：x叉乘PX = 0
// |yp2 - p1| |0|
// |p0 - xp2| X = |0|
// |xp1 - yp0| |0|
// 两个点:关键帧从世界坐标系到相机坐标系的旋转和平移矩阵
// |yp2 - p1 | |0|
// |p0 - xp2 | X = |0| ===> AX = 0
// |y'p2' - p1' | |0|
// |p0' - x'p2'| |0|
// 变成程序中的形式：
// |xp2 - p0 | |0|
// |yp2 - p1 | X = |0| ===> AX = 0
// |x'p2'- p0'| |0|
// |y'p2'- p1'| |0|
// 然后就组成了一个四元一次正定方程组，SVD求解，右奇异矩阵的最后一行就是最终的解.
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
// 归一化之前的检查
if(x3D.at<float>(3)==0)
continue;
// 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}
else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)
{
// 如果是双目，用视差角更大的那个双目信息来恢复，直接用已知3D点反投影了
x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);
}
else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)
{
x3D = pKF2->UnprojectStereo(idx2);
}
else
continue; //No stereo and very low parallax, 放弃
// 为方便后续计算，转换成为了行向量
cv::Mat x3Dt = x3D.t();
关键帧从世界坐标系到相机坐标系的旋转和平移矩阵
//Check triangulation in front of cameras
// Step 6.5：检测生成的3D点是否在相机前方,不在的话就放弃这个点
float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
if(z1<=0)
continue;
float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
if(z2<=0)
continue;
计算3D点，并检测生成的3D点是否在相机的前方
//Check reprojection error in first keyframe
// Step 6.6：计算3D点在当前关键帧下的重投影误差
const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
const float invz1 = 1.0/z1;
if(!bStereo1)
{
// 单目情况下
float u1 = fx1*x1*invz1+cx1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
// 假设测量有一个像素的偏差，2自由度卡方检验阈值是5.991
if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
continue;
}
else
{
// 双目情况
float u1 = fx1*x1*invz1+cx1;
// 根据视差公式计算假想的右目坐标
float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
float errX1_r = u1_r - kp1_ur;
// 自由度为3，卡方检验阈值是7.8
if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
continue;
}
//Check reprojection error in second keyframe
// 计算3D点在另一个关键帧下的重投影误差，操作同上
const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
const float invz2 = 1.0/z2;
if(!bStereo2)
{
float u2 = fx2*x2*invz2+cx2;
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
continue;
}
else
{
float u2 = fx2*x2*invz2+cx2;
float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
float errX2_r = u2_r - kp2_ur;
if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
continue;
}
首先将3D点从世界坐标系下变换到当前关键帧对应的相机坐标系下，然后通过对应的相机内参投影到当前关键帧中，计算该点和关键帧中真实点对应点的距离，检测误差是否满足卡方检验，不满足则认为该地图点（3D点）无效，然后对共视关键帧做类似处理。
//Check scale consistency
// Step 6.7：检查尺度连续性
// 世界坐标系下，3D点与相机间的向量，方向由相机指向3D点
cv::Mat normal1 = x3D-Ow1;
float dist1 = cv::norm(normal1);
cv::Mat normal2 = x3D-Ow2;
float dist2 = cv::norm(normal2);
if(dist1==0 || dist2==0)
continue;
// ratioDist是不考虑金字塔尺度下的距离比例
const float ratioDist = dist2/dist1;
// 金字塔尺度因子的比例
const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];
/*if(fabs(ratioDist-ratioOctave)>ratioFactor)
continue;*/
// 距离的比例和图像金字塔的比例不应该差太多，否则就跳过
if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
continue;
// Triangulation is succesfull
// Step 6.8：三角化生成3D点成功，构造成MapPoint
MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
// Step 6.9：为该MapPoint添加属性：
// a.观测到该MapPoint的关键帧
pMP->AddObservation(mpCurrentKeyFrame,idx1);
pMP->AddObservation(pKF2,idx2);
mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
pKF2->AddMapPoint(pMP,idx2);
// b.该MapPoint的描述子
pMP->ComputeDistinctiveDescriptors();
// c.该MapPoint的平均观测方向和深度范围
pMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pMP);
// Step 6.10：将新产生的点放入检测队列
// 这些MapPoints都会经过MapPointCulling函数的检验
mlpRecentAddedMapPoints.push_back(pMP);
nnew++;
最后检查尺度连续性。如果通过了层层检查，则用3D点构建地图点，并为其添加属性，与当前关键帧和其共视关键帧建立联系。
至此，LocalMapping::CreateNewMapPoints()函数结束，回到LocalMapping::Run()中
// 已经处理完队列中的最后的一个关键帧
if(!CheckNewKeyFrames())
{
// Find more matches in neighbor keyframes and fuse point duplications
// Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
SearchInNeighbors();
}
如果已经处理完队列中的最后的一个关键帧，则进入到SearchInNeighbors()函数中：
void LocalMapping::SearchInNeighbors()
{
// Retrieve neighbor keyframes
// Step 1：获得当前关键帧在共视图中权重排名前nn的邻接关键帧
// 开始之前先定义几个概念
// 当前关键帧的邻接关键帧，称为一级相邻关键帧，也就是邻居
// 与一级相邻关键帧相邻的关键帧，称为二级相邻关键帧，也就是邻居的邻居
// 单目情况要20个邻接关键帧，双目或者RGBD则要10个
int nn = 10;
if(mbMonocular)
nn=20;
// 和当前关键帧相邻的关键帧，也就是一级相邻关键帧
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
// Step 2：存储一级相邻关键帧及其二级相邻关键帧
vector<KeyFrame*> vpTargetKFs;
// 开始对所有候选的一级关键帧展开遍历：
for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
// 没有和当前帧进行过融合的操作
if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
continue;
// 加入一级相邻关键帧
vpTargetKFs.push_back(pKFi);
// 标记已经加入
pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;
// Extend to some second neighbors
// 以一级相邻关键帧的共视关系最好的5个相邻关键帧 作为二级相邻关键帧
const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
// 遍历得到的二级相邻关键帧
for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
{
KeyFrame* pKFi2 = *vit2;
// 当然这个二级相邻关键帧要求没有和当前关键帧发生融合,并且这个二级相邻关键帧也不是当前关键帧
if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
continue;
// 存入二级相邻关键帧
vpTargetKFs.push_back(pKFi2);
}
}
首先定义参数，单目情况要20个邻接关键帧，双目或者RGBD则要10个。得到当前关键帧的20个共视程度最高共视关键帧，称为一级共视关键帧，如果该一级共视关键帧没有和当前关键帧做过融合，则将其存储到vpTargetKFs变量中，并将其进行标记，表示不再需要融合，接着得到和每一个一级共视关键帧的共视程度最高的5个关键帧，称为二级共视关键帧，如果该二级共视关键帧没有和当前关键帧做过融合，则将其存储到vpTargetKFs变量中，并将其进行标记。
// Search matches by projection from current KF in target KFs
// 使用默认参数, 最优和次优比例0.6,匹配时检查特征点的旋转
ORBmatcher matcher;
// Step 3：将当前帧的地图点分别与一级二级相邻关键帧地图点进行融合 -- 正向
vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
// 将地图点投影到关键帧中进行匹配和融合；融合策略如下
// 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
// 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点
// 注意这个时候对地图点融合的操作是立即生效的
matcher.Fuse(pKFi,vpMapPointMatches);
}
首先取出当前帧中的地图点，再和当前关键帧的一二级共视关键帧用matcher.Fuse(pKFi,vpMapPointMatches)分别融合。下面，进入matcher.Fuse()函数中：
int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)
{
cv::Mat Rcw = pKF->GetRotation();
cv::Mat tcw = pKF->GetTranslation();
const float &fx = pKF->fx;
const float &fy = pKF->fy;
const float &cx = pKF->cx;
const float &cy = pKF->cy;
const float &bf = pKF->mbf;
cv::Mat Ow = pKF->GetCameraCenter();
int nFused=0;
const int nMPs = vpMapPoints.size();
// 遍历所有的待投影地图点
for(int i=0; i<nMPs; i++)
{
// Step 1 判断地图点的有效性
MapPoint* pMP = vpMapPoints[i];
if(!pMP)
continue;
// 地图点无效 或 已经是该帧的地图点（无需融合），跳过
if(pMP->isBad() || pMP->IsInKeyFrame(pKF))
continue;
// 将地图点变换到关键帧的相机坐标系下
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc = Rcw*p3Dw + tcw;
// Depth must be positive
// 深度值为负，跳过
if(p3Dc.at<float>(2)<0.0f)
continue;
首先，得到当前关键帧（实际上为共视关键帧）从世界坐标系到相机坐标系的旋转和平移矩阵，得到当前关键帧光心在世界坐标系中的坐标、内参。然后遍历地图点，并判断地图点是否有效，将有效的地图点变换到当前关键帧的相机坐标系下，记为 ( u , v ) (u,v) (u,v)，判断其深度是否为负值。
// Step 2 得到地图点投影到关键帧的图像坐标
const float invz = 1/p3Dc.at<float>(2);
const float x = p3Dc.at<float>(0)*invz;
const float y = p3Dc.at<float>(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;
// Point must be inside the image
// 投影点需要在有效范围内
if(!pKF->IsInImage(u,v))
continue;
const float ur = u-bf*invz;
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
cv::Mat PO = p3Dw-Ow;
const float dist3D = cv::norm(PO);
// Depth must be inside the scale pyramid of the image
// Step 3 地图点到关键帧相机光心距离需满足在有效范围内
if(dist3D<minDistance || dist3D>maxDistance )
continue;
// Viewing angle must be less than 60 deg
// Step 4 地图点的平均观测方向（正视程度）要小于60°
cv::Mat Pn = pMP->GetNormal();
if(PO.dot(Pn)<0.5*dist3D)
continue;
int nPredictedLevel = pMP->PredictScale(dist3D,pKF);
// Search in a radius
// 根据MapPoint的深度确定尺度，从而确定搜索范围
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
// Step 5 在投影点附近搜索窗口内找到候选匹配点的索引
const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
将地图点投影到关键帧的图像坐标中，判断是否在有效范围内。然后计算最大距离和最小距离，以及地图点到相机光心的距离，并判断地图点到相机光心的距离（在世界坐标系下求得）是否在最大距离和最小距离之间。判断地图点的平均观测方向（正视程度）要小于60°。进一步求解出搜索范围radius，在投影点 ( u , v ) (u,v) (u,v)附近的radius范围内进行搜索，得到匹配点的索引并判断是否成功搜索到。
// Match to the most similar keypoint in the radius
// Step 6 遍历寻找最佳匹配点（最小描述子距离）
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = 256;
int bestIdx = -1;
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)// 步骤3：遍历搜索范围内的features
{
const size_t idx = *vit;
const cv::KeyPoint &kp = pKF->mvKeysUn[idx];
const int &kpLevel= kp.octave;
// 金字塔层级要接近（同一层或小一层），否则跳过
if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
continue;
// 计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过
if(pKF->mvuRight[idx]>=0)
{
// Check reprojection error in stereo
// 双目情况
const float &kpx = kp.pt.x;
const float &kpy = kp.pt.y;
const float &kpr = pKF->mvuRight[idx];
const float ex = u-kpx;
const float ey = v-kpy;
// 右目数据的偏差也要考虑进去
const float er = ur-kpr;
const float e2 = ex*ex+ey*ey+er*er;
//自由度为3, 误差小于1个像素,这种事情95%发生的概率对应卡方检验阈值为7.82
if(e2*pKF->mvInvLevelSigma2[kpLevel]>7.8)
continue;
}
else
{
// 单目情况
const float &kpx = kp.pt.x;
const float &kpy = kp.pt.y;
const float ex = u-kpx;
const float ey = v-kpy;
const float e2 = ex*ex+ey*ey;
// 自由度为2的，卡方检验阈值5.99（假设测量有一个像素的偏差）
if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99)
continue;
}
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(dist<bestDist)// 找MapPoint在该区域最佳匹配的特征点
{
bestDist = dist;
bestIdx = idx;
}
}
首先得到地图点的描述子，创建bestDist和bestIdx分别保存最佳匹配距离和最佳匹配点的索引。然后遍历每一个匹配点，先判断其金字塔层级是否和地图点的投影点一致，进一步计算匹配点和 ( u , v ) (u,v) (u,v)的距离，并判断是否满足卡方分布。取出每一个匹配点的描述子并和地图点的描述子比较，计算两者的汉明距离，用bestDist和bestIdx分别保存最佳匹配距离和最佳匹配点的索引。
// If there is already a MapPoint replace otherwise add new measurement
// Step 7 找到投影点对应的最佳匹配特征点，根据是否存在地图点来融合
// 最佳匹配距离要小于阈值
if(bestDist<=TH_LOW)
{
MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
if(pMPinKF)
{
// 如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换
if(!pMPinKF->isBad())
{
if(pMPinKF->Observations()>pMP->Observations())
pMP->Replace(pMPinKF);
else
pMPinKF->Replace(pMP);
}
}
else
{
// 如果最佳匹配点没有对应地图点，添加观测信息
pMP->AddObservation(pKF,bestIdx);
pKF->AddMapPoint(pMP,bestIdx);
}
nFused++;
}
}
return nFused;
先判断最佳匹配距离是否小于阈值，然后判断最佳匹配点是否有对应有效的地图点，如果有，选择被观测次数最多的那个进行替换，如果没有，则给该匹配点增加观测信息。最后返回融合成功的数目。
返回到LocalMapping::SearchInNeighbors()函数中：
// Search matches by projection from target KFs in current KF
// Step 4：将一级二级相邻关键帧地图点分别与当前关键帧地图点进行融合 -- 反向
// 用于进行存储要融合的一级邻接和二级邻接关键帧所有MapPoints的集合
vector<MapPoint*> vpFuseCandidates;
vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());
// Step 4.1：遍历每一个一级邻接和二级邻接关键帧，收集他们的地图点存储到 vpFuseCandidates
for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
{
KeyFrame* pKFi = *vitKF;
vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();
// 遍历当前一级邻接和二级邻接关键帧中所有的MapPoints,找出需要进行融合的并且加入到集合中
for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
{
MapPoint* pMP = *vitMP;
if(!pMP)
continue;
// 如果地图点是坏点，或者已经加进集合vpFuseCandidates，跳过
if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
continue;
// 加入集合，并标记已经加入
pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
vpFuseCandidates.push_back(pMP);
}
}
// Step 4.2：进行地图点投影融合,和正向融合操作是完全相同的
// 不同的是正向操作是"每个关键帧和当前关键帧的地图点进行融合",而这里的是"当前关键帧和所有邻接关键帧的地图点进行融合"
matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
进行地图点投影融合,和正向融合操作是完全相同的，不同的是正向操作是"每个关键帧和当前关键帧的地图点进行融合"，而这里的是"当前关键帧和所有邻接关键帧的地图点进行融合"。
// Update points
// Step 5：更新当前帧地图点的描述子、深度、观测主方向等属性
vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
{
MapPoint* pMP=vpMapPointMatches[i];
if(pMP)
{
if(!pMP->isBad())
{
// 在所有找到pMP的关键帧中，获得最佳的描述子
pMP->ComputeDistinctiveDescriptors();
// 更新平均观测方向和观测距离
pMP->UpdateNormalAndDepth();
}
}
}
// Update connections in covisibility graph
// Step 6：更新当前帧的MapPoints后更新与其它帧的连接关系
// 更新covisibility图
mpCurrentKeyFrame->UpdateConnections();
更新当前关键帧地图点的描述子、深度、观测主方向等属性，并通过 mpCurrentKeyFrame->UpdateConnections()更新共视图，该函数在前面讲解过，在此不再赘述。
至此，SearchInNeighbors()函数结束，重新跳转回LocalMapping::Run()中：
// 终止BA的标志
mbAbortBA = false;
// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
if(!CheckNewKeyFrames() && !stopRequested())
{
// Local BA
// Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BA
if(mpMap->KeyFramesInMap()>2)
// 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
将终止BA的标志置为false，表示允许BA，如果已经处理完队列的最后一个关键帧，并且回环检测也没有请求停止LocalMapping，进一步判断局部地图中的关键帧是否大于两个，如果是，则进行局部BA，下面进入到Optimizer::LocalBundleAdjustment()函数中
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{
// 该优化函数用于LocalMapping线程的局部BA优化
// Local KeyFrames: First Breadth Search from Current Keyframe
list<KeyFrame*> lLocalKeyFrames;
// Step 1 将当前关键帧及其共视关键帧加入lLocalKeyFrames
lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;
// 找到关键帧连接的共视关键帧（一级相连），加入lLocalKeyFrames中
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
{
KeyFrame* pKFi = vNeighKFs[i];
// 把参与局部BA的每一个关键帧的 mnBALocalForKF设置为当前关键帧的mnId，防止重复添加
pKFi->mnBALocalForKF = pKF->mnId;
// 保证该关键帧有效才能加入
if(!pKFi->isBad())
lLocalKeyFrames.push_back(pKFi);
}
// Local MapPoints seen in Local KeyFrames
// Step 2 遍历 lLocalKeyFrames 中(一级)关键帧，将它们观测的MapPoints加入到lLocalMapPoints
list<MapPoint*> lLocalMapPoints;
// 遍历 lLocalKeyFrames 中的每一个关键帧
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
// 取出该关键帧对应的地图点
vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
// 遍历这个关键帧观测到的每一个地图点，加入到lLocalMapPoints
for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
{
if(!pMP->isBad()) //保证地图点有效
// 把参与局部BA的每一个地图点的 mnBALocalForKF设置为当前关键帧的mnId
// mnBALocalForKF 是为了防止重复添加
if(pMP->mnBALocalForKF!=pKF->mnId)
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
} // 判断这个地图点是否靠谱
} // 遍历这个关键帧观测到的每一个地图点
} // 遍历 lLocalKeyFrames 中的每一个关键帧
将当前关键帧及其(一级)共视关键帧加入lLocalKeyFrames中，遍历 lLocalKeyFrames 中的关键帧（当前关键帧及其共视关键帧），将它们观测的MapPoints加入到lLocalMapPoints，为了防止重复添加，把参与局部BA的每一个关键帧的 mnBALocalForKF设置为当前关键帧的mnId，把参与局部BA的每一个地图点的 mnBALocalForKF设置为当前关键帧的mnId。
// Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
// Step 3 得到能被局部MapPoints观测到，但不属于局部关键帧的(二级)关键帧，这些关键帧在局部BA优化时不优化
list<KeyFrame*> lFixedCameras;
// 遍历局部地图中的每个地图点
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
// 观测到该MapPoint的KF和该MapPoint在KF中的索引
map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
// 遍历所有观测到该地图点的关键帧
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
// pKFi->mnBALocalForKF!=pKF->mnId 表示不属于局部关键帧，
// pKFi->mnBAFixedForKF!=pKF->mnId 表示还未标记为fixed（固定的）关键帧
if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
{
// 将局部地图点能观测到的、但是不属于局部BA范围的关键帧的mnBAFixedForKF标记为pKF（触发局部BA的当前关键帧）的mnId
pKFi->mnBAFixedForKF=pKF->mnId;
if(!pKFi->isBad())
lFixedCameras.push_back(pKFi);
}
}
}
得到能被局部MapPoints观测到，但不属于局部关键帧的关键帧（即二级关键帧），这些关键帧在局部BA优化时不优化， 放入lFixedCameras中，对优化提出约束条件。
接下来使用g2o开始局部BA：
// Setup optimizer
// Step 4 构造g2o优化器，按照步骤来就行了
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// LM大法好
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// 外界设置的停止优化标志
// 可能在 Tracking::NeedNewKeyFrame() 里置位
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
// 记录参与局部BA的最大关键帧mnId
unsigned long maxKFid = 0;
// Set Local KeyFrame vertices
// Step 5 添加待优化的位姿顶点：Pose of Local KeyFrame
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();//VertexSE3Expmap（李代数位姿）
// 设置初始优化位姿
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));//SE3Quat：这个是g2o中定义的相机位姿的类
vSE3->setId(pKFi->mnId);
// 如果是初始关键帧，要锁住位姿不优化
vSE3->setFixed(pKFi->mnId==0);
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
设置优化器，优化方式采用列文伯格-马夸尔特方法(LM)，设置停止优化标志pbStopFlag，用maxKFid记录参与局部BA的最大关键帧mnId，添加待优化的位姿顶点，遍历lLocalKeyFrames中的每一个关键帧，设置初始优化位姿和id，如果是初始关键帧，要锁住位姿不优化，其为参考位姿，maxKFid 记录所有关键帧中的最大id。
// Set Fixed KeyFrame vertices
// Step 6 添加不优化的位姿顶点：Pose of Fixed KeyFrame，注意这里调用了vSE3->setFixed(true)
// 不优化为啥也要添加？回答：为了增加约束信息
for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
// 所有的这些顶点的位姿都不优化，只是为了增加约束项
vSE3->setFixed(true);
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
添加不优化的位姿顶点，增加约束信息。
// Set MapPoint vertices
// Step 7 添加待优化的3D地图点顶点
// 边的数目 = pose数目 * 地图点数目
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);
vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
vpEdgesStereo.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFStereo;
vpEdgeKFStereo.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeStereo;
vpMapPointEdgeStereo.reserve(nExpectedSize);
// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
const float thHuberMono = sqrt(5.991);
// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
const float thHuberStereo = sqrt(7.815);
// 遍历所有的局部地图中的地图点
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
// 添加顶点：MapPoint
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
// 前面记录maxKFid的作用在这里体现
int id = pMP->mnId+maxKFid+1;
vPoint->setId(id);
// 因为使用了LinearSolverType，所以需要将所有的三维点边缘化掉
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
// 观测到该地图点的KF和该地图点在KF中的索引
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
// Set edges
// Step 8 在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边
// 遍历所有观测到当前地图点的关键帧
for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(!pKFi->isBad())
{
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
// 根据单目/双目两种不同的输入构造不同的误差边
// Monocular observation
// 单目模式下
if(pKFi->mvuRight[mit->second]<0)
{
Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id))); //id为地图点的索引
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);
// 权重为特征点所在图像金字塔的层数的倒数
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
// 使用鲁棒核函数抑制外点
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKFi);
vpMapPointEdgeMono.push_back(pMP);
}
else // Stereo observation
{
Eigen::Matrix<double,3,1> obs;
const float kp_ur = pKFi->mvuRight[mit->second];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberStereo);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
e->bf = pKFi->mbf;
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vpEdgeKFStereo.push_back(pKFi);
vpMapPointEdgeStereo.push_back(pMP);
}
}
} // 遍历所有观测到当前地图点的关键帧
} // 遍历所有的局部地图中的地图点
// 开始BA前再次确认是否有外部请求停止优化，因为这个变量是引用传递，会随外部变化
// 可能在 Tracking::NeedNewKeyFrame(), mpLocalMapper->InsertKeyFrame 里置位
if(pbStopFlag)
if(*pbStopFlag)
return;
边的数目 = pose数目 * 地图点数目，设置边、关键帧和地图点的存储变量，显著性水平为0.05的卡方分布的对应的临界阈值，因为使用了LinearSolverType，所以需要将所有的三维点边缘化掉，并将地图点添加到顶点中。在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边。开始BA前再次确认是否有外部请求停止优化，因为这个变量是引用传递，会随外部变化
下面，正式开始BA优化
// Step 9 开始优化。分成两个阶段
// 第一阶段优化
optimizer.initializeOptimization();
// 迭代5次
optimizer.optimize(5);
bool bDoMore= true;
// 检查是否外部请求停止
if(pbStopFlag)
if(*pbStopFlag)
bDoMore = false;
// 如果有外部请求停止,那么就不在进行第二阶段的优化
if(bDoMore)
{
// Check inlier observations
// Step 10 检测outlier，并设置下次不优化
// 遍历所有的单目误差边
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
// 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，不优化了。
if(e->chi2()>5.991 || !e->isDepthPositive())
{
// 不优化
e->setLevel(1);
}
// 第二阶段优化的时候就属于精求解了,所以就不使用核函数
e->setRobustKernel(0);
}
// 对于所有的双目的误差边也都进行类似的操作
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
{
g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
MapPoint* pMP = vpMapPointEdgeStereo[i];
if(pMP->isBad())
continue;
// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
if(e->chi2()>7.815 || !e->isDepthPositive())
{
e->setLevel(1);
}
e->setRobustKernel(0);
}
优化分两步，第一步优化进行五次，然后检查是否外部请求停止，如果有外部请求停止,那么就不进行第二阶段的优化，遍历所有的单目误差边，如果发现外点，则退出循环，如果不满足卡方分布，也不优化，并关闭核函数。
// Optimize again without the outliers
// Step 11：排除误差较大的outlier后再次优化 -- 第二阶段优化
optimizer.initializeOptimization(0);
optimizer.optimize(10);
}
vector<pair<KeyFrame*,MapPoint*> > vToErase;
vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
// Check inlier observations
// Step 12：在优化后重新计算误差，剔除连接误差比较大的关键帧和MapPoint
// 对于单目误差边
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
// 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，要删掉了
if(e->chi2()>5.991 || !e->isDepthPositive())
{
// outlier
KeyFrame* pKFi = vpEdgeKFMono[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
// 双目误差边
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
{
g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
MapPoint* pMP = vpMapPointEdgeStereo[i];
if(pMP->isBad())
continue;
if(e->chi2()>7.815 || !e->isDepthPositive())
{
KeyFrame* pKFi = vpEdgeKFStereo[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
// Get Map Mutex
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
// 删除点
// 连接偏差比较大，在关键帧中剔除对该地图点的观测
// 连接偏差比较大，在地图点中剔除对该关键帧的观测
if(!vToErase.empty())
{
for(size_t i=0;i<vToErase.size();i++)
{
KeyFrame* pKFi = vToErase[i].first;
MapPoint* pMPi = vToErase[i].second;
pKFi->EraseMapPointMatch(pMPi);
pMPi->EraseObservation(pKFi);
}
}
排除误差较大的外点之后后进行第二步优化，迭代十次，在优化后重新计算误差，剔除连接误差比较大的关键帧和MapPoint。
// Recover optimized data
// Step 13：优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性
//Keyframes
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKF = *lit;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
pKF->SetPose(Converter::toCvMat(SE3quat));
}
//Points
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
}
优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性。
至此，LocalBundleAdjustment()函数结束。
下面继续回到LocalMapping::Run()函数中，进入KeyFrameCulling()函数中：
void LocalMapping::KeyFrameCulling()
{
// Check redundant keyframes (only local keyframes)
// A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
// in at least other 3 keyframes (in the same or finer scale)
// We only consider close stereo points
// 该函数里变量层层深入，这里列一下：
// mpCurrentKeyFrame：当前关键帧，本程序就是判断它是否需要删除
// pKF： mpCurrentKeyFrame的某一个共视关键帧
// vpMapPoints：pKF对应的所有地图点
// pMP：vpMapPoints中的某个地图点
// observations：所有能观测到pMP的关键帧
// pKFi：observations中的某个关键帧
// scaleLeveli：pKFi的金字塔尺度
// scaleLevel：pKF的金字塔尺度
// Step 1：根据共视图提取当前关键帧的所有共视关键帧
vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
// 对所有的共视关键帧进行遍历
for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
{
KeyFrame* pKF = *vit;
if(pKF->mnId==0)
continue;
// Step 2：提取每个共视关键帧的地图点
const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
// 记录某个点被观测次数，后面并未使用
int nObs = 3;
// 观测次数阈值，默认为3
const int thObs=nObs;
// 记录冗余观测点的数目
int nRedundantObservations=0;
int nMPs=0;
// Step 3：遍历该共视关键帧的所有地图点，判断是否90%以上的地图点能被其它至少3个关键帧（同样或者更低层级）观测到
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad())
{
if(!mbMonocular)
{
// 对于双目，仅考虑近处（不超过基线的 35倍 ）的地图点
if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
continue;
}
nMPs++;
// pMP->Observations() 是观测到该地图点的相机总数目（单目1，双目2）
if(pMP->Observations()>thObs)
{
const int &scaleLevel = pKF->mvKeysUn[i].octave;
// Observation存储的是可以看到该地图点的所有关键帧的集合
const map<KeyFrame*, size_t> observations = pMP->GetObservations();
int nObs=0;
// 遍历观测到该地图点的关键帧
for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(pKFi==pKF)
continue;
const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
// 尺度约束：为什么pKF 尺度+1 要大于等于 pKFi 尺度？
// 回答：因为同样或更低金字塔层级的地图点更准确
if(scaleLeveli<=scaleLevel+1)
{
nObs++;
// 已经找到3个满足条件的关键帧，就停止不找了
if(nObs>=thObs)
break;
}
}
// 地图点至少被3个关键帧观测到，就记录为冗余点，更新冗余点计数数目
if(nObs>=thObs)
{
nRedundantObservations++;
}
}
}
}
}
// Step 4：该关键帧90%以上的有效地图点被判断为冗余的，则删除该关键帧
if(nRedundantObservations>0.9*nMPs)
pKF->SetBadFlag();
}
}
检测并剔除当前关键帧的共视关键帧中冗余的共视关键帧，冗余的判定：该共视关键帧的90%的地图点可以被其它至少三个共视关键帧观测到。
然后回到LocalMapping::Run()函数中：
// Step 8 将当前帧加入到闭环检测队列中
// 注意这里的关键帧被设置成为了bad的情况,这个需要注意
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
else if(Stop()) // 当要终止当前线程的时候
{
// Safe area to stop
while(isStopped() && !CheckFinish())
{
// 如果还没有结束利索,那么等
// usleep(3000);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
}
// 然后确定终止了就跳出这个线程的主循环
if(CheckFinish())
break;
}
// 查看是否有复位线程的请求
ResetIfRequested();
// Tracking will see that Local Mapping is not busy
SetAcceptKeyFrames(true);
// 如果当前线程已经结束了就跳出主循环
if(CheckFinish())
break;
//usleep(3000);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
}
// 设置线程已经终止
SetFinish();
至此，Local Mapping线程全部结束。
代码注释参考：https://github.com/electech6/ORBSLAM2_detailed_comments
1475
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
