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函数中引入的不合格MapPointsMapPointCulling();// Triangulate new MapPoints// VI-C new map points creation// 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPointsCreateNewMapPoints();// 已经处理完队列中的最后的一个关键帧if(!CheckNewKeyFrames()){// Find more matches in neighbor keyframes and fuse point duplications// 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPointsSearchInNeighbors();}// 终止BA的标志mbAbortBA = false;// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMappingif(!CheckNewKeyFrames() && !stopRequested()){// VI-D Local BA// 当局部地图中的关键帧大于2个的时候进行局部地图的BAif(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 stopwhile(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 busySetAcceptKeyFrames(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 MapPointslist<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;// 观测阈值int nThObs;if(mbMonocular)nThObs = 2;elsenThObs = 3;const int cnThObs = nThObs;// 遍历等待检查的MapPointswhile(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);elselit++;}
}

LocalMapping::CreateNewMapPoints()

// 相机运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints
// 程序有点长,有点耐心
void LocalMapping::CreateNewMapPoints()
{// Retrieve neighbor keyframes in covisibility graph// 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图int nn = 10;if(mbMonocular)nn=20;// Step 1：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFsconst 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：遍历相邻关键帧vpNeighKFsfor(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中存放着双目的深度值，如果不是双目，其值将为-1const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];bool bStereo1 = kp1_ur>=0;// 当前匹配在邻接关键帧中的特征点const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];// mvuRight中存放着双目的深度值，如果不是双目，其值将为-1const 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);}elsecontinue; //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;// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）自由度2if((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;// 自由度为3if((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点成功，构造成MapPointMapPoint* 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 neighborsconst 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是否为坏点，或者是否已经加进集合vpFuseCandidatesif(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：提取每个共视关键帧的MapPointsconst vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();int nObs = 3;                       // 接下来程序中使用到的循环变量,记录了某个点的被观测次数 (其实这里初始化是没有意义的)const int thObs=nObs;               // 观测阈值,默认为3int 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 * fxif(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

ORB_SLAM2探秘 第二章相关推荐

1. 第二章 指南（4.3）添加 View

   原文:Adding a view 作者:Rick Anderson 翻译:魏美娟(初见) 校对:赵亮(悲梦).高嵩(Jack).娄宇(Lyrics).许登洋(Seay).姚阿勇(Dr.Yao) 本节将 ...

2. 【JVM】第二章 JVM类加载、JVM对象

   第二章 JVM类加载.JVM对象 文章目录 第二章 JVM类加载.JVM对象 一.JVM类加载 1.介绍 2.类加载器 3.类加载机制 4.类加载过程 二.JVM对象 1.对象的内存布局 2.对象的访 ...

3. 王道考研 计算机网络笔记 第二章：物理层

   本文基于2019 王道考研 计算机网络: 2019 王道考研 计算机网络 个人笔记总结 第一章:王道考研 计算机网络笔记 第一章:概述&计算机网络体系结构 后续章节将陆续更新- 第二章 一.物 ...

4. 计算机组成原理-第二章 数据表示与运算

   计算机组成原理-第二章 数据表示与运算 一.数据的表示 1.数值型数据的表示(重点难点) 1.1数值型数据的表示--进位制 1.2数值型数据表示-码制 1.3数值型数据的表示--定点数 1.4数值型数 ...

5. 2021-08-08概率论与数理统计-第二章

   文章目录 概率论与数理统计-第二章 概率论与数理统计-第二章

6. 软件构造 第二章 第一节 软件生命周期和版本控制

   软件构造第二章 第一节 软件生命周期和版本控制 基本内容 Software Development Lifecycle (SDLC) Traditional software process mode ...

7. 第二节认识计算机教案,第二章 第二节 局域网的构建 教学设计_博客

   <第二章 第二节 局域网的构建 教学设计_博客>由会员分享,可在线阅读,更多相关<第二章 第二节 局域网的构建 教学设计_博客(3页珍藏版)>请在装配图网上搜索. 1.第二章 ...

8. ArcGIS for Desktop入门教程_第二章_Desktop简介 - ArcGIS知乎-新一代ArcGIS问答社区

   原文:ArcGIS for Desktop入门教程_第二章_Desktop简介 - ArcGIS知乎-新一代ArcGIS问答社区 1 Desktop简介 1.1 ArcGIS for Desktop ...

9. 第二章 序列比对——Needleman-Wunsch全局比对

   [生信]第二章 序列比对--Needleman-Wunsch全局比对 主要为基因组测序比对相关知识,部分内容作笔记自查使用.如有错误或遗漏还请海涵,可评论或邮箱联系. 最后修改时间:2020-04-0 ...

10. 第二章 序列比对——Blast局部比对

    第二章 序列比对--Blast局部比对  阅读量: 330 主要为基因组测序比对相关知识,部分内容作笔记自查使用.如有错误或遗漏还请海涵,可评论或邮箱联系. 最后修改时间:2020-04-16 16: ...

最新文章

1. Docker_Swarm集群系统
2. 精度首超ResNet，港科大和CMU提出史上最强二值化网络ReActNet
3. C# VS中类的开头自动生成
4. VUEJS-checkbox全选全不选
5. websphere一直安装部署_WebSphere集群安装配置及部署应用说明
6. 上市4年，现在苹果要停产它了
7. Spring 框架学习 有用
8. python绘制散点图，非常全，非常详细（已验证）
9. 带grub的软盘镜像制作
10. 人工智能：智能优化算法
11. 模似html元素tilte提示
12. 摄影测量（tip1）：空间前方交会与空间后方交会
13. 2023最新SSM计算机毕业设计选题大全（附源码+LW）之java古诗词知识学习系统q5697
14. 几年前的项目用了YYYY-MM-dd，跨年夜老板喊我回去改Bug
15. nodejs爬虫与python爬虫_Python，Node.js 哪个比较适合写爬虫？
16. 2022，这些大屏可视化素材，够你用一整年了（附可视化素材包）
17. 潘石屹这回是真的卖掉了“根”
18. android图片下载到本地
19. jdk1.8的安装环境配置详细步骤
20. 人工智能将在未来几年，呈现以下四个主要发展趋势

热门文章

1. Java中的指针---Unsafe应用解析
2. Scalaz（49）－ scalaz-stream: 深入了解-Sink/Channel
3. SharePoint 设置Library中文档的默认打开方式
4. Boolean.getBoolean()和Boolean.parseBoolean()
5. 几种常见的程序命名规则
6. mysql必知必会的数据_MySQL必知必会---数据过滤
7. lgg8各个版本_LG正式推出G8SThinQ 搭载骁龙855
8. mybatis-plus主键生成策略
9. c语言自动化课题设计,自动化专业C语言程序设计课堂教学方案设计和实践.doc
10. php讲一个正整数前补0,php 数字补零的两种的简单示例