??局部建圖線程入口：可執行程序在初始化三個線程的時候，在System.cc的構造函數中進入局部建圖線程

    mpLocalMapper = new LocalMapping(mpMap, 				//指定使iomanipmSensor==MONOCULAR);	// TODO 為什么這個要設置成為MONOCULAR？？？//運行這個局部建圖線程mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,	//這個線程會調用的函數mpLocalMapper);				//這個調用函數的參數

插入關鍵幀

??將關鍵幀插入到關鍵幀列表中

void LocalMapping::InsertKeyFrame(KeyFrame *pKF)

// 插入關鍵幀,由外部（Tracking）線程調用;這里只是插入到列表中,等待線程主函數對其進行處理
void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
{unique_lock<mutex> lock(mMutexNewKFs);// 將關鍵幀插入到列表中mlNewKeyFrames.push_back(pKF);mbAbortBA=true;
}

?
?

處理關鍵幀

void LocalMapping::ProcessNewKeyFrame()

??Tracking線程將關鍵幀插入了隊列中，該函數取出一個關鍵幀進行處理：建立路標點與關鍵幀的聯系，更新共視圖，將關鍵幀插入地圖中。

    {unique_lock<mutex> lock(mMutexNewKFs);// 取出列表中最前面的關鍵幀，作為當前要處理的關鍵幀mpCurrentKeyFrame = mlNewKeyFrames.front();// 取出最前面的關鍵幀后，在原來的列表里刪掉該關鍵幀mlNewKeyFrames.pop_front();}

計算關鍵幀的詞袋信息

??會更新關鍵幀數據庫（存儲所有關鍵幀的詞典）。

mpCurrentKeyFrame->ComputeBoW();

建立當前幀與路標點的聯系

??在追蹤局部地圖時，只是建立了特征點和路標點間的匹配關系，并沒有建立聯系。

    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); }}}} 

更新關鍵幀間的聯系（共視圖）

mpCurrentKeyFrame->UpdateConnections();

關鍵幀插入地圖

mpMap->AddKeyFrame(mpCurrentKeyFrame);

?
?

剔除錯誤路標點

mlpRecentAddedMapPoints：當前關鍵幀中待檢測的地圖點集合。

void LocalMapping::MapPointCulling()

剔除條件1

** 地圖點被追蹤的次數/預計被看到的次數<0.25**
** 地圖點被追蹤的次數指的是：地圖點投影到關鍵幀中，和多少關鍵幀的特征點形成了匹配。
預計被看到的次數指的是：該地圖點在多少幀的視野范圍內。**

        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);}

剔除條件2

該路標點可以被3個以上的關鍵幀觀測到，否則剔除

        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()，僅從隊列中刪除，放棄繼續對該MapPoint的檢測lit = mlpRecentAddedMapPoints.erase(lit);elselit++;

?
?

檢查關鍵幀列表是否為空

bool LocalMapping::CheckNewKeyFrames()
{unique_lock<mutex> lock(mMutexNewKFs);return(!mlNewKeyFrames.empty());
}

創建新地圖點

獲得數據

（1）取出與當前幀共視程度最高的幾個關鍵幀。

 const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

（2）獲取當前關鍵幀的變換矩陣:由旋轉矩陣和平移矩陣組成

    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));

（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）判斷是否需要對每個相鄰的關鍵進行三角化,需要滿足一定的稀疏性。單目應滿足的條件：
兩幀間基線的長度要大于路標點平均深度的一個比例。

        else    {// 單目相機情況// 鄰接關鍵幀的場景深度中值const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);// baseline與景深的比例const float ratioBaselineDepth = baseline/medianDepthKF2;// 如果比例特別小，基線太短恢復3D點不準，那么跳過當前鄰接的關鍵幀，不生成3D點if(ratioBaselineDepth<0.01)continue;}

（2）根據兩個關鍵幀的位姿計算它們之間的基礎矩陣

cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);

（3）通過BoW對兩關鍵幀的未匹配的特征點快速匹配，并使用極線約束

        vector<pair<size_t,size_t> > vMatchedIndices;matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);

對每對匹配通過三角化生成3D點

（1）獲得視差角

            // 歸一化坐標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));

（2）如果視差角滿足約束，則使用SVD分解進行三角化，求出路標點的世界坐標

            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);}

（3）判斷三角化后的路標點是否滿足約束
1）深度大于0
2）重投影誤差小于閾值

            // 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.991if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)continue;}

建立新的地圖點與幀、地圖間的聯系

地圖點放入隊列中，等待檢測

??

融合相鄰幀中的路標點

??因為在局部建圖線程中，創建了很多新的路標點，因此需要進行融合。

獲得一階、二階共視關鍵幀

    // 開始之前先定義幾個概念// 當前關鍵幀的鄰接關鍵幀，稱為一級相鄰關鍵幀，也就是鄰居// 與一級相鄰關鍵幀相鄰的關鍵幀，稱為二級相鄰關鍵幀，也就是鄰居的鄰居// 單目情況要10個鄰接關鍵幀，雙目或者RGBD則要20個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);}}

進行路標點融合

	// 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：遍歷每一個一級鄰接和二級鄰接關鍵幀，收集他們的地圖點存儲到 vpFuseCandidatesfor(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);

更新當前幀路標點的特性

    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();}}}

更新共視圖

mpCurrentKeyFrame->UpdateConnections();

?
?

局部地圖BA優化

Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

??對局部地圖進行全局優化。
??優化變量：當前關鍵幀以及與當前關鍵幀具有一定共視關系的關鍵幀的位姿、路標點坐標。
??那些能觀測到路標點，但無法達到公式關系閾值的關鍵幀，會提供約束，但不會進行優化。

?
?

檢測冗余的關鍵幀

??檢測當前關鍵幀在共視圖中的關鍵幀，根據地圖點在共視圖中的冗余程度剔除該共視關鍵幀。
??冗余關鍵幀的判定：90%以上的地圖點能被其他關鍵幀（至少3個）觀測到。

void LocalMapping::KeyFrameCulling()

獲得公式關鍵幀

vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();

遍歷共視關鍵幀

（1）遍歷某一共視關鍵幀的所有地圖點
（2）如果該共視關鍵幀中的路標點的90%以上被3個關鍵幀觀測到，則該公式關鍵幀視為冗余關鍵幀，刪除該關鍵幀

?
?

總結

?l?局部建圖線程在可執行程序初始化時進入，直接調用LocalMapping::Run函數。
流程為：

（1）先將標志位置為FALSE，使得Tracking線程產生關鍵幀時，不再送入局部建圖線程中。

SetAcceptKeyFrames(false);

（2）在隊列mlNewKeyFrames中取出一個關鍵幀，進行處理

ProcessNewKeyFrame();

1）計算關鍵幀的詞袋，更新關鍵幀數據庫。
2）在追蹤局部地圖時，匹配了局部地圖中的路標點和關鍵幀的特征點，但未建立聯系。因此建立關鍵幀的路標點與該關鍵幀的聯系。同時將路標點加入到待檢測變量中()
3）更新共視圖（只是更新當前幀及其有共視關系的關鍵幀間的權重）。
4）將關鍵幀插入到地圖中。

（3）去除錯誤路標點

MapPointCulling();

遍歷待檢測的地圖點，去除錯誤地圖點的條件為：
1） 地圖點被追蹤的次數/預計被看到的次數<0.25
2）該路標點可以被3個以上的關鍵幀觀測到，否則剔除
（4）創建新的地圖點

CreateNewMapPoints();

如果不創建新的地圖點的話，已知深度的地圖點會越來越少，以至于后面無法繼續追蹤。
1）找到與當前幀共視程度最高的n個關鍵幀
2）遍歷所有候選關鍵幀，使用詞袋加速算法+極線約束尋找匹配的特征點
3）對每對特征點進行三角化測距
4）建立路標點與關鍵幀、局部地圖的聯系
（5）重復特征點的融合
創建了新的路標點之后，與這個路標點1匹配的特征點之前可能有匹配的路標點2。這是需要在路標點1、路標點2中進行選擇。

SearchInNeighbors();

1）尋找當前關鍵幀的候選關鍵幀（共視點多）
2）將當前幀的路標點與每個候選關鍵幀的特征點進行重新匹配。
2）將所有候選關鍵幀的路標點與當前幀的特征點進行重新匹配。
（6）全局BA優化

Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

優化的是當前關鍵幀及其共視關鍵幀的位姿與路標點的坐標。這里的共視關鍵幀指的是共視路標點的數目超過一個閾值。對于那些共視路標點沒有超過閾值的關鍵幀，會為BA優化提供約束，但并不進行優化。
（7）刪除冗余關鍵幀

KeyFrameCulling();

冗余關鍵幀指的是路標點的90%以上被3個以上的關鍵幀觀察到。

總結

以上是生活随笔為你收集整理的ORB_SLAM2局部建图线程的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。