LocalMapping
LocalMapping::Run()
// 线程主函数
void LocalMapping::Run()
{
// 设置当前run函数正在运行
mbFinished = false;
// 主循环
while(1)
{
// Tracking will see that Local Mapping is busy
// 告诉Tracking，LocalMapping正处于繁忙状态，
// LocalMapping线程处理的关键帧都是Tracking线程发过的
// 在LocalMapping线程还没有处理完关键帧之前Tracking线程最好不要发送太快
SetAcceptKeyFrames(false);
// Check if there are keyframes in the queue
// 等待处理的关键帧列表不为空
if(CheckNewKeyFrames())
{
// BoW conversion and insertion in Map
// VI-A keyframe insertion
// 计算关键帧特征点的BoW映射，将关键帧插入地图
ProcessNewKeyFrame();
// Check recent MapPoints
// VI-B recent map points culling
// 剔除ProcessNewKeyFrame函数中引入的不合格MapPoints
MapPointCulling();
// Triangulate new MapPoints
// VI-C new map points creation
// 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints
CreateNewMapPoints();
// 已经处理完队列中的最后的一个关键帧
if(!CheckNewKeyFrames())
{
// Find more matches in neighbor keyframes and fuse point duplications
// 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
SearchInNeighbors();
}
// 终止BA的标志
mbAbortBA = false;
// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
if(!CheckNewKeyFrames() && !stopRequested())
{
// VI-D Local BA
// 当局部地图中的关键帧大于2个的时候进行局部地图的BA
if(mpMap->KeyFramesInMap()>2)
// 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化的时候,这个优化函数也能够及时地注意到
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// Check redundant local Keyframes
// VI-E local keyframes culling
// 检测并剔除当前帧相邻的关键帧中冗余的关键帧
// 剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到
// trick!
// Tracking中先把关键帧交给LocalMapping线程
// 并且在Tracking中InsertKeyFrame函数的条件比较松，交给LocalMapping线程的关键帧会比较密
// 在这里再删除冗余的关键帧
// 也是本文的创新点之一吧(guoqing)
KeyFrameCulling();
}
// 将当前帧加入到闭环检测队列中
// 注意这里不排除一开始的时候插入的关键帧是阔以的,但是在后来被设置成为了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();
}
LocalMapping::ProcessNewKeyFrame()
/*
* @brief 处理列表中的关键帧
* - 计算Bow，加速三角化新的MapPoints
* - 关联当前关键帧至MapPoints，并更新MapPoints的平均观测方向和观测距离范围
* - 插入关键帧，更新Covisibility图和Essential图
* @see VI-A keyframe insertion
*
*/
void LocalMapping::ProcessNewKeyFrame()
{
// Step 1：从缓冲队列中取出一帧关键帧 (这个函数设计的也是一次只能够处理一个关键帧)
// Tracking线程向LocalMapping中插入关键帧存在该队列中
{
unique_lock<mutex> lock(mMutexNewKFs);
// 从列表中获得一个等待被插入的关键帧
// 获取list中第一个元素，之前需要检查list是否为空，函数CheckNewKeyFrames()检查此list是否为空
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：跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定
// 在TrackLocalMap函数中将局部地图中的MapPoints与当前帧进行了匹配，
// 但没有对这些匹配上的MapPoints与当前帧进行关联
const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
// 对当前处理的这个关键帧中的所有的地图点展开遍历
for(size_t i=0; i<vpMapPointMatches.size(); i++)
{
MapPoint* pMP = vpMapPointMatches[i];
if(pMP)
{
if(!pMP->isBad())
{
// 非当前帧生成的MapPoints
// 为当前帧在tracking过程跟踪到的MapPoints更新属性
if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
{
// 添加观测
pMP->AddObservation(mpCurrentKeyFrame, i);
// 获得该点的平均观测方向和观测距离范围
pMP->UpdateNormalAndDepth();
// 加入关键帧后，更新3d点的最佳描述子
pMP->ComputeDistinctiveDescriptors();
}
else // this can only happen for new stereo points inserted by the Tracking
{
// 这种情况对应着,当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息
// 当前帧生成的MapPoints
// 将双目或RGBD跟踪过程中新插入的MapPoints放入mlpRecentAddedMapPoints，等待检查
// CreateNewMapPoints函数中通过三角化也会生成MapPoints
// 这些MapPoints都会经过MapPointCulling函数的检验
mlpRecentAddedMapPoints.push_back(pMP); // 认为这些由当前关键帧生成的地图点不靠谱,将其加入到待检查的地图点列表中
}
}
}
}
// Update links in the Covisibility Graph
// Step 4：更新关键帧间的连接关系，Covisibility图和Essential图(tree)
mpCurrentKeyFrame->UpdateConnections();
// Insert Keyframe in Map
// Step 5：将该关键帧插入到地图中
mpMap->AddKeyFrame(mpCurrentKeyFrame);
}
LocalMapping::MapPointCulling()
// 剔除 ProcessNewKeyFrame 和 CreateNewMapPoints 函数中引入的质量不好的 MapPoints
void LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints
list<MapPoint*>::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())
{
// Step 1：已经是坏点的MapPoints直接从检查链表中删除
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(pMP->GetFoundRatio()<0.25f)
{
// Step 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)
{
// Step 3：将不满足VI-B条件的MapPoint剔除
// VI-B 条件2：从该点建立开始，到现在已经过了不小于2个关键帧
// 但是观测到该点的关键帧数却不超过cnThObs帧，那么该点检验不合格
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
// Step 4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
// 因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测
lit = mlpRecentAddedMapPoints.erase(lit);
else
lit++;
}
}
LocalMapping::CreateNewMapPoints()
// 相机运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints
// 程序有点长,有点耐心
void LocalMapping::CreateNewMapPoints()
{
// Retrieve neighbor keyframes in covisibility graph
// 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图
int nn = 10;
if(mbMonocular)
nn=20;
// Step 1：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
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也不清楚,猜测还是各种经验值吧
const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
// 三角化成功的地图点的计数
int nnew=0;
// 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点
// 导致这个结果有两个因素:一个还是两个关键帧之间的基线太短,另外一个就是相邻关键帧看到的空间的尺度非常大
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; //一般相邻的关键帧应该不会有这么大的视角吧,90°多,不过感觉这个也不好说啊;不过下面的程序中认为如果视差角的余弦值大于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);
// step 6.4：三角化恢复3D点
cv::Mat x3D;
// cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常
// cosParallaxRays < cosParallaxStereo 表明视差角很小
// 视差角度小时用三角法恢复3D点，视差角大时用双目恢复3D点（双目以及深度有效）
if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
{
// Linear Triangulation Method
// 见Initializer.cpp的 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;
// Euclidean coordinates 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}
else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2) // 视差大的时候使用双目信息来恢复 - 直接反投影了
{
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
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
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;*/
// ratioDist*ratioFactor < ratioOctave 或 ratioDist/ratioOctave > ratioFactor表明尺度变化是连续的
//? 还不是非常明白,感觉这里的意思大致应该就是, 深度值的比例和图像金字塔的比例不应该差太多
// ratioDist < (ratioOctave/ratioFactor) , ratioDist > (ratioOctave*ratioFactor) ,中间那一段不行
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的关键帧
// b.该MapPoint的描述子
// c.该MapPoint的平均观测方向和深度范围
pMP->AddObservation(mpCurrentKeyFrame,idx1);
pMP->AddObservation(pKF2,idx2);
mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
pKF2->AddMapPoint(pMP,idx2);
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pMP);
// step 6.10：将新产生的点放入检测队列
// 这些MapPoints都会经过MapPointCulling函数的检验
mlpRecentAddedMapPoints.push_back(pMP);
nnew++;
}
}
}
LocalMapping::SearchInNeighbors()
// 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
void LocalMapping::SearchInNeighbors()
{
// Retrieve neighbor keyframes
// STEP 1：获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧
// 找到当前帧一级相邻与二级相邻关键帧
// 单目情况要10个邻接关键帧，双目或者RGBD则要20个
int nn = 10;
if(mbMonocular)
nn=20;
// 候选的一级相邻关键帧
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
// 筛选之后的一级相邻关键帧以及二级相邻关键帧
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
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 2：将当前帧的MapPoints分别与一级二级相邻帧(的MapPoints)进行融合 -- 正向
vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
// 投影当前帧的MapPoints到相邻关键帧pKFi中，并判断是否有重复的MapPoints
// 1.如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
// 2.如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint
// 注意这个时候对地图点融合的操作是立即生效的
matcher.Fuse(pKFi,vpMapPointMatches);
}
// Search matches by projection from target KFs in current KF
// 用于进行存储要融合的一级邻接和二级邻接关键帧所有MapPoints的集合
vector<MapPoint*> vpFuseCandidates;
vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());
// STEP 3：将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合 -- 反向
// 遍历每一个一级邻接和二级邻接关键帧
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;
// 判断MapPoints是否为坏点，或者是否已经加进集合vpFuseCandidates
if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
continue;
// 加入集合，并标记已经加入
pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
vpFuseCandidates.push_back(pMP);
}
}
// 进行融合操作,其实这里的操作和上面的那个融合操作是完全相同的,不过上个是"每个关键帧和当前关键帧的地图点进行融合",而这里的是"当前关键帧和所有邻接关键帧的地图点进行融合"
matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
// Update points
// STEP4：更新当前帧MapPoints的描述子，深度，观测主方向等属性
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
// STEP5：更新当前帧的MapPoints后更新与其它帧的连接关系
// 更新covisibility图
mpCurrentKeyFrame->UpdateConnections();
}
LocalMapping::KeyFrameCulling()
// 关键帧剔除,在Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。
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
// STEP1：根据Covisibility Graph提取当前帧的共视关键帧 (所有)
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;
// STEP2：提取每个共视关键帧的MapPoints
const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
int nObs = 3; // 接下来程序中使用到的循环变量,记录了某个点的被观测次数 (其实这里初始化是没有意义的)
const int thObs=nObs; // 观测阈值,默认为3
int nRedundantObservations=0; // 冗余观测地图点的计数器
// 不是说我从地图点中得到了其观测数据我就信了,这里还要求这个地图点在当前关键帧和在邻接关键帧中的特征尺度变化不太大才可以
// 认为这个地图点被"冗余观测"了
int nMPs=0; // 计数器,参与到检测的地图点的总数目
// STEP3：遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad())
{
if(!mbMonocular)
{
// 对于双目，仅考虑近处的MapPoints，不超过mbf * 35 / fx
// 不过配置文件中给的是近点的定义是 40 * fx
if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
continue;
}
nMPs++;
// MapPoints至少被三个关键帧观测到
if(pMP->Observations()>thObs)
{
const int &scaleLevel = pKF->mvKeysUn[i].octave;
const map<KeyFrame*, size_t> observations = pMP->GetObservations();
// 判断该MapPoint是否同时被三个关键帧观测到
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;
// Scale Condition
// 尺度约束，要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
if(scaleLeveli<=scaleLevel+1)
{
nObs++;
// 已经找到三个同尺度的关键帧可以观测到该MapPoint，不用继续找了
if(nObs>=thObs)
break;
}
}
// 该MapPoint至少被三个关键帧观测到
if(nObs>=thObs)
{
nRedundantObservations++;
}
}
}
}
}
// STEP4：该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧
if(nRedundantObservations>0.9*nMPs)
// 剔除的时候就设置一个 bad flag 就可以了
pKF->SetBadFlag();
}
}
REFERENCES
ORB_SLAM2代码阅读(3)——LocalMapping线程 https://blog.csdn.net/u014709760/article/details/91378039
扫一扫
336
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
