ORB-SLAM2源码——LocalMapping.cc
文章目录
这一部分的源码主要涉及的是局部建图线程相关的部分。
1. 局部建图线程介绍
关键帧来自于跟踪线程。然后剔除不靠谱的关键帧，再利用关键帧之间的共视关系生成新的地图点，再对多个关键帧和它们的地图点进行局部BA优化（比较慢），但是因为局部建图是个独立的子线程，不影响跟踪的实时性。最后剔除冗余的关键帧，方便后续的闭环检测的实时性。
局部建图的范围只有关键帧，而且是按照共视关系来处理（在跟踪线程中定下来的）关键帧。
1.1.局部建图的流程
1.1.1. LocalMapping::Run 函数对应局部建图线程的代码
步骤：
- 设置接收关键帧为 false，告诉Tracking，LocalMapping正处于繁忙状态，不发送关键帧。
- 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
- 根据地图点的观测情况剔除质量不好的地图点
- 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
- 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
- 当局部地图中的关键帧大于2个的时候进行局部地图的BA
- 检测并剔除当前帧相邻的关键帧中冗余的关键帧
- 将当前帧加入到闭环检测队列中
// 线程主函数
void LocalMapping::Run()
{
// 标记状态，表示当前run函数正在运行，尚未结束
mbFinished = false;
// 主循环
while(1)
{
// Tracking will see that Local Mapping is busy
// Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我
// LocalMapping线程处理的关键帧都是Tracking线程发来的
SetAcceptKeyFrames(false);
// 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);
}
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();
}
2. 局部建图线程函数中处理新关键帧、剔除和新生成地图点涉及的函数及原理
Run 中主要是这几个函数：
// Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
ProcessNewKeyFrame();
// Step 3 根据地图点的观测情况剔除质量不好的地图点
MapPointCulling();
// Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
CreateNewMapPoints();
// 已经处理完队列中的最后的一个关键帧
if(!CheckNewKeyFrames())
{
// Find more matches in neighbor keyframes and fuse point duplications
// Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
SearchInNeighbors();
}
2.1. 处理新的关键帧：
局部建图线程中的关键帧来自跟踪线程。这些关键帧会进入一个缓存队列，等待局部建图线程的处理，包括计算词袋向量，更新观测、描述子、共视图，插入到地图中等。
2.1.1. ProcessNewKeyFrame函数处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
/**
* @brief 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
*
*/
void LocalMapping::ProcessNewKeyFrame()
{
// Step 1：从缓冲队列中取出一帧关键帧
// 该关键帧队列是Tracking线程向LocalMapping中插入的关键帧组成
{
unique_lock<mutex> lock(mMutexNewKFs);
// 取出列表中最前面的关键帧，作为当前要处理的关键帧
mpCurrentKeyFrame = mlNewKeyFrames.front();
// 取出最前面的关键帧后，在原来的列表里删掉该关键帧
// pop_front()用于从列表容器的开头弹出（删除）元素，该容器的第二个元素成为第一个元素，并且该容器中的第一个元素从该容器中删除。
mlNewKeyFrames.pop_front();
}
// Compute Bags of Words structures
// Step 2：计算该关键帧特征点的词袋向量
mpCurrentKeyFrame->ComputeBoW();
// Associate MapPoints to the new keyframe and update normal and descriptor
// Step 3：当前处理关键帧中有效的地图点，更新normal，描述子等信息
// TrackLocalMap中和当前帧新匹配上的地图点和当前关键帧进行关联绑定，GetMapPointMatches 获取当前关键帧的具体的地图点
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);
}
}
}
}
// Update links in the Covisibility Graph
// Step 4：更新关键帧间的连接关系（共视图）
mpCurrentKeyFrame->UpdateConnections();
// Insert Keyframe in Map
// Step 5：将该关键帧插入到地图中
mpMap->AddKeyFrame(mpCurrentKeyFrame);
}
2.2. 剔除不合格的地图点
ORB-SLAM2中的新增地图点需要经过比较严苛的筛查才能留下，这不仅可以提高定位与建图的准确性,还能控制地图规模，降低计算量，使得ORB-SLAM2可以在较大的场景中运行。其中，新增地图点主要来自两个地方：
①在处理新关键帧时，在双目相机或RGB-D相机模式下跟踪线程中新产生的地图点；
②局部建图线程中共视的关键帧之间生成新的地图点。
这些新增地图点只要满足如下两个条件之一就会被剔除。
- 条件一：跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%。
- 条件二：从该点建立开始，到现在已经超过了2个关键帧，但是观测到该点的相机数目却不超过阈值 cnThObs。这个阈值表示观测到地图点的相机数目，对于单目相机来说为2，对于双目相机或RGB-D相机来说则为3。而使用单目相机代码实现如下。观测一次，观测到地图点的相机数目加1;使用双目相机或RGB-D相机观测一次，观测到地图点的相机数目加2。
- 注：比例和设定的阈值均为经验参数
2.2.1. MapPointCulling函数检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点
/**
* @brief 检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点
* mlpRecentAddedMapPoints：存储新增的地图点，这里是要删除其中不靠谱的
*/
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：遍历检查新添加的地图点
while(lit!=mlpRecentAddedMapPoints.end())
{
MapPoint* pMP = *lit;
if(pMP->isBad())
{
// Step 2.1：已经是坏点的地图点仅从队列中删除
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(pMP->GetFoundRatio()<0.25f)
{
// Step 2.2：跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于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)
{
// Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧
// 但是观测到该点的相机数却不超过阈值cnThObs，从地图中删除
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
// Step 2.4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
// 因此没有SetBadFlag()，仅从队列中删除
lit = mlpRecentAddedMapPoints.erase(lit);
else
lit++;
}
}
2.3. 生成新的地图点
在局部建图线程中，会在共视关键帧之间重新进行特征匹配、三角化，生成新的地图点。
首先明确这里是在当前关键帧和它的共视关键帧之间进行匹配和三角化的，当前帧的共视关键帧数目很多，但并不是都能参与三角化。首先，这会带来非常大的计算量；其次，如果这些关键帧距离太近，则产生的视差会很小，三角化结果会很不准确。因此，对于参与三角化的关键帧有一定要求，就是它们之间的基线（光心之间的距离）要超过一定的阈值。
这个阈值该如何确定：
- 对于双目相机来说，它本身的物理结构中就包含了基线，也就是**双目相机的左右目光心距离。**如果关键帧之间的基线比双目相机的基线还小，则相机自身的左右目三角化精度都比关键帧三角化的精度高，可放弃对该关键帧的三角化。
- 对于单目相机来说，因为没有物理基线作为参考，只能退而求其次。先求出当前关键帧所有地图点深度中值，然后判断关键帧之间的基线和中值的比值，如果比值小于 0.01，则认为关键帧之间的基线太小，三角化得到的三维点会很不准确，放弃该关键帧的三角化。如下图12-2所示。
在双目相机模式下生成三维点会相对复杂一些。如图12-3所示，p1，p2是两个匹配的特征点，分别来自双目相机在不同位置时的左目相机。根据针孔相机投影模型分别得到 p1，p2在各自相机坐标系下的归一化坐标，然后用各自位姿下的旋转向量将其旋转到世界坐标系下，得到射线1和射线2。假设射线1和射线2之间的夹角为Θ。
此时还不能直接用p1，p2来三角化得到三维点，还需要考虑双目相机本身也可以通过左右目匹配得到三维点。即到底是用关键帧三角化还是左右目匹配。
如图12-4所示，绿色的点是双目相机在不同位置时通过左右目匹配的三维点，Θ1，Θ2分别是双目相机观测该三维点时的夹角；红色的点是通过双目相机在不同位置时左目相机中的匹配点对p1，p2三角化得到的三维点，观测三维点的夹角为Θ。如果Θ >max(Θ1,Θ2)，则用左目匹配（对应红色线）三角化得到三维点，否则用双目相机本身的左右目匹配（对应蓝色线）得到三维点。
2.3.1. CreateNewMapPoints 函数用当前关键帧与相邻关键帧通过三角化产生新的地图点
/**
* @brief 用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
*
*/
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();
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;
// Search matches with epipolar restriction and triangulate
// Step 2：遍历相邻关键帧，搜索匹配并用极线约束剔除误匹配，最终三角化
for(size_t i=0; i<vpNeighKFs.size(); i++)
{
// ! 疑似bug，正确应该是 if(i>0 && !CheckNewKeyFrames())
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;
// 基线长度：norm函数是求2-范数的。
const float baseline = cv::norm(vBaseline);
// Step 3：判断相机运动的基线是不是足够长
if(!mbMonocular)
{
// 如果是双目相机，关键帧间距小于本身的基线时不生成3D点
// 因为太短的基线下能够恢复的地图点不稳定
if(baseline<pKF2->mb)
continue;
}
else
{
// 单目相机情况
// 相邻关键帧的场景深度中值
const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
// 基线与景深的比例
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：通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对
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;
// 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;
// cosParallaxStereo1、cosParallaxStereo2在后面可能不存在，需要初始化为较大的值
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]));
// 得到双目观测的视差角中cos值最小的那个。角度值大的，cos值小。
cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
// Step 6.4：三角化恢复3D点
cv::Mat x3D;
// cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常,0.9998 对应1°
// cosParallaxRays < cosParallaxStereo 表明两帧匹配点对夹角大于双目本身观察三维点夹角，因为角度大，cos值小
// 匹配点对夹角大，用三角法恢复3D点
// 参考：https://github.com/raulmur/ORB_SLAM2/issues/345
if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
{
// Linear Triangulation Method
// 见Initializer.cc的 Triangulate 函数,实现是一样的,顶多就是把投影矩阵换成了变换矩阵
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);
}
// 匹配点对夹角小，用双目恢复3D点
else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)
{
// 如果是双目，用视差角更大的那个双目信息来恢复，直接用已知3D点反投影了
// KeyFrame::UnprojectStereo 函数是在双目和RGBD情况下将特征点反投影到空间中得到世界坐标系下三维点
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;
//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;
}
//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++;
}
}
}
其中的函数 ORBmatcher::SearchForTriangulation 用于通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对。代码部分可以参考博客ORB-SLAM2源码学习篇（2）——ORBmatcher.cc代码的学习。
2.4. 检查并融合当前关键帧与相邻帧的地图点
在局部建图线程中，地图点产生了比较大的变动，比如前面在共视关键帧之间重新进行特征匹配、三角化，生成新的地图点；又如前面根据一定的规则剔除不合格的地图点，这时就需要对已有的地图点进行整理。
包括合并重复的地图点，用更准确的地图点替换旧的地图点，最后统一更新地图点的描述子、深度、平均观测方向等属性。具体步骤如下：
2.4.1. SearchInNeighbors 函数检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点
注意：正向投影融合和反向投影融合有区别。
正向操作是"每个关键帧和当前关键帧的地图点进行融合",而反向操作是"当前关键帧和所有邻接关键帧的地图点进行融合"
/**
* @brief 检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点
*/
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);
}
}
// 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);
}
// 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：更新当前帧与其它帧的共视连接关系
mpCurrentKeyFrame->UpdateConnections();
}
其中的函数 ORBmatcher::Fuse 用于将地图点投影到关键帧中进行匹配和融合。当然返回的是融合的地图点的个数，用于后续的判断。但是因为融合的时候进行了相似地图点观测数目的比较以及地图点的替换，所以并不影响融合的结果。代码部分可以参考博客ORB-SLAM2源码学习篇（2）——ORBmatcher.cc代码的学习。
其函数的融合策略如下：
- 如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
- 如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点
2.5. 关键帧的剔除
在跟踪线程中插入关键帧的条件是相对宽松的，目的是增加跟踪过程的弹性，在大旋转、快速运动、纹理不足等恶劣情况下可以提高跟踪的成功率。这些关键帧会传递到局部建图线程中，同时还有大量的共识关键帧，此时大量关键帧会使得局部BA（Local BA 优化）变得慢，因此需要及时剔除冗余程度较高的关键帧，冗余关键帧也是在共视关键帧中查找的。
冗余关键帧的判定是其90%以上的地图点能被其它至少3个关键帧观测到。
剔除掉多余的关键帧也不会对地图产生多少影响，还可以减少局部BA优化的规模。
2.5.1. KeyFrameCulling 函数检测当前关键帧在共视图中的关键帧，根据地图点在共视图中的冗余程度剔除该共视关键帧
冗余关键帧的判定：90%以上的地图点能被其他关键帧（至少3个）观测到。
其中需要统计的地图点，是那些满足金字塔尺度约束条件的近点。
该函数里变量存在嵌套，这里列一下：
- mpCurrentKeyFrame：当前关键帧，本程序就是判断它是否需要删除
- pKF： mpCurrentKeyFrame的某一个共视关键帧
- vpMapPoints：pKF对应的所有地图点
- pMP：vpMapPoints中的某个地图点
- observations：所有能观测到pMP的关键帧
- pKFi：observations中的某个关键帧
- scaleLeveli：pKFi的金字塔尺度
- scaleLevel：pKF的金字塔尺度
/**
* @brief 检测当前关键帧在共视图中的关键帧，根据地图点在共视图中的冗余程度剔除该共视关键帧
*/
void LocalMapping::KeyFrameCulling()
{
// Check redundant keyframes (only local keyframes)
// 只考虑近地图点（近点）
// Step 1：根据共视图提取当前关键帧的所有共视关键帧
vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
// 对所有的共视关键帧进行遍历
for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
{
// pKF 是当前关键帧的某一个共视关键帧
KeyFrame* pKF = *vit;
// 第1个关键帧不能删除，跳过
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：遍历该共视关键帧的所有地图点，其中能被其它至少3个关键帧观测到的地图点为冗余地图点
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad())
{
if(!mbMonocular)
{
// 对于双目或RGB-D，仅考虑近处（不超过基线的40倍 ）的地图点
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里面存储的是可以看到该地图点的所有关键帧的集合，以及这些能观测到该地图点的关键帧上面对应这个地图点的二维特征点的索引 Idx。
//（挺复杂的，需要多看看，理解！！！）
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;
// 取出 pKFi 对应的金字塔尺度
const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
// 尺度约束：为什么在比较 pKF 和 pKFi 的金字塔尺度是，pKF 的尺度+1 要大于等于 pKFi 尺度？
// 回答：金字塔尺度约束条件：
// 要求观测到该地图点的其它关键帧对应的二维特征点所在的金字塔层级（scaleLeveli）小于或等于当前帧中该地图点对应的二维特征点所在的金字塔层级（scaleLevel）+1。
// 因为同样或更低金字塔层级，图像越大，分辨率越高，对应观测到的地图点更准确
if(scaleLeveli<=scaleLevel+1)
{
nObs++; // n0bs 观测到地图点的关键帧的数目
// 已经找到3个满足条件的关键帧，就停止不找了
if(nObs>=thObs)
break;
}
}
// 地图点至少被3个关键帧观测到，就记录为冗余点，更新冗余点计数数目
if(nObs>=thObs)
{
nRedundantObservations++;
}
}
}
}
}
// Step 4：如果该关键帧90%以上的有效地图点被判断为冗余的，则认为该关键帧是冗余的，需要删除该关键帧
if(nRedundantObservations>0.9*nMPs)
pKF->SetBadFlag();
}
}