針對MapPoint的生命周期,我們關心以下3個問題:

?創建MapPoint的時機:
Tracking線程中初始化過程(Tracking::MonocularInitialization()和Tracking::StereoInitialization())
Tracking線程中創建新的關鍵幀(Tracking::CreateNewKeyFrame())
Tracking線程中恒速運動模型跟蹤(Tracking::TrackWithMotionModel())也會產生臨時地圖點,但這些臨時地圖點在跟蹤成功后會被馬上刪除(那跟蹤失敗怎么辦?跟蹤失敗的話不會產生關鍵幀,這些地圖點也不會被注冊進地圖).
LocalMapping線程中創建新地圖點的步驟(LocalMapping::CreateNewMapPoints())會將當前關鍵幀與前一關鍵幀進行匹配,生成新地圖點.

刪除MapPoint的時機:
LocalMapping線程中刪除惡劣地圖點的步驟(LocalMapping::MapPointCulling()).
刪除關鍵幀的函數KeyFrame::SetBadFlag()會調用函數MapPoint::EraseObservation()刪除地圖點對關鍵幀的觀測,若地圖點對關鍵幀的觀測少于2,則地圖點無法被三角化,就刪除該地圖點.

替換MapPoint的時機:
LoopClosing線程中閉環矯正(LoopClosing::CorrectLoop())時當前關鍵幀和閉環關鍵幀上的地圖點發生沖突時,會使用閉環關鍵幀的地圖點替換當前關鍵幀的地圖點.
LoopClosing線程中閉環矯正函數LoopClosing::CorrectLoop()會調用LoopClosing::SearchAndFuse()將閉環關鍵幀的共視關鍵幀組中所有地圖點投影到當前關鍵幀的共視關鍵幀組中,發生沖突時就會替換.

4. ORB-SLAM2代碼詳解04_幀Frame

4.1 各成員函數/變量

Frame類與相機相關的參數大部分設為static類型,整個系統內的所有Frame對象共享同一份相機參數.

這些參數首先由Tracking對象從配置文件TUM1.yaml內讀入,再傳給Frame類的構造函數,第一次調用Frame的構造函數時為這些成員變量賦值.

Tracking::Tracking(const string &strSettingPath, ...) {
?// 從配置文件中讀取相機參數并構造內參矩陣cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);float fx = fSettings["Camera.fx"];float fy = fSettings["Camera.fy"];float cx = fSettings["Camera.cx"];float cy = fSettings["Camera.cy"];
?cv::Mat K = cv::Mat::eye(3, 3, CV_32F);K.at<float>(0, 0) = fx;K.at<float>(1, 1) = fy;K.at<float>(0, 2) = cx;K.at<float>(1, 2) = cy;K.copyTo(mK);
?// ...
}
?
// 每傳來一幀圖像,就調用一次該函數
cv::Mat Tracking::GrabImageStereo(..., const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp) {mCurrentFrame = Frame(mImGray, mK, mDistCoef, mbf, mThDepth);
?Track();
?// ...
}
?
// Frame構造函數
Frame::Frame(cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth): mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {// ...// 第一次調用Frame()構造函數時為所有static變量賦值if (mbInitialComputations) {fx = K.at<float>(0, 0);fy = K.at<float>(1, 1);cx = K.at<float>(0, 2);cy = K.at<float>(1, 2);invfx = 1.0f / fx;invfy = 1.0f / fy;// ...mbInitialComputations = false;      // 賦值完畢后將mbInitialComputations復位}
?mb = mbf / fx;
}

4.2 特征點提取

在Frame類構造函數中調用成員變量mpORBextractorLeft和mpORBextractorRight的()運算符進行特征點提取.

mvKeys、 mvKeysUn、 mvuRight、 mvDepth的坐標索引是對應的,也就是說對于第i個圖像特征點:

其畸變矯正前的左目特征點是mvKeys[i].
其畸變矯正后的左目特征點是mvKeysUn[i].
其在右目圖片中對應特征點的橫坐標為mvuRight[i],縱坐標與mvKeys[i]的縱坐標相同.
特征點的深度是mvDepth[i].
對于單目特征點(單目相機輸入的特征點或沒有找到右目匹配的左目圖像特征點),其mvuRight和mvDepth均為-1.

4.2.1 特征點提取:?ExtractORB()

void Frame::ExtractORB(int flag, const cv::Mat &im) {if (flag == 0)      // flag==0, 表示對左圖提取ORB特征點(*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);else                // flag==1, 表示對右圖提取ORB特征點(*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, mDescriptorsRight);
}

4.3 ORB-SLAM2對雙目/RGBD特征點的預處理

雙目/RGBD相機中可以得到特征點的立體信息,包括右目特征點信息(mvuRight)、特征點深度信息(mvDepth)

對于雙目相機,通過雙目特征點匹配關系計算特征點的深度值.
對于RGBD相機,根據特征點深度構造虛擬的右目圖像特征點.

通過這種預處理,在后面SLAM系統的其他部分中不再區分雙目特征點和RGBD特征點,它們以雙目特征點的形式被處理.(僅通過判斷mvuRight[idx]判斷某特征點是否有深度).

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint *> &vpMapPoints, const float th) {// ...for (size_t idx : vIndices) {if (F.mvuRight[idx] > 0) {      // 通過判斷 mvuRight[idx] 判斷該特征點是否有深度// 針對有深度的特征點特殊處理} else {// 針對單目特征點的特殊處理}}// ...
}

4.4 雙目視差公式

觀測距離,基線,焦距,視差,根據三角形相似性:

?

得到:

4.5 雙目特征點的處理:雙目圖像特征點匹配:?ComputeStereoMatches()

?稀疏立體匹配原理
函數ComputeStereoMatches()
兩幀圖像稀疏立體匹配
*輸入：兩幀立體矯正后的圖像對應的orb特征點集
*過程：
1.行特征點統計
2.粗匹配
3.精確匹配SAD
4.亞像素精度優化
5.最有視差值/深度選擇
6.刪除離缺點（outliers）
*輸出：稀疏特征點視差圖/深度圖和匹配結果

雙目相機分別提取到左右目特征點后對特征點進行雙目匹配,并通過雙目視差估計特征點深度.雙目特征點匹配步驟:

粗匹配: 根據特征點描述子距離和金字塔層級判斷匹配.粗匹配關系是按行尋找的,對于左目圖像中每個特征點,在右目圖像對應行上尋找匹配特征點.
精匹配: 根據特征點周圍窗口內容相似度判斷匹配.
亞像素插值: 將特征點相似度與匹配坐標之間擬合成二次曲線,尋找最佳匹配位置(得到的是一個小數).
記錄右目匹配mvuRight和深度mvDepth信息.
離群點篩選: 以平均相似度的2.1倍為標準,篩選離群點.

?亞像素插值

?

void Frame::ComputeStereoMatches()
{/*兩幀圖像稀疏立體匹配（即：ORB特征點匹配，非逐像素的密集匹配，但依然滿足行對齊）* 輸入：兩幀立體矯正后的圖像img_left 和 img_right 對應的orb特征點集* 過程：1. 行特征點統計. 統計img_right每一行上的ORB特征點集，便于使用立體匹配思路(行搜索/極線搜索）進行同名點搜索, 避免逐像素的判斷.2. 粗匹配. 根據步驟1的結果，對img_left第i行的orb特征點pi，在img_right的第i行上的orb特征點集中搜索相似orb特征點, 得到qi3. 精確匹配. 以點qi為中心，半徑為r的范圍內，進行塊匹配（歸一化SAD），進一步優化匹配結果4. 亞像素精度優化. 步驟3得到的視差為uchar/int類型精度，并不一定是真實視差，通過亞像素差值（拋物線插值)獲取float精度的真實視差5. 最優視差值/深度選擇. 通過勝者為王算法（WTA）獲取最佳匹配點。6. 刪除離群點(outliers). 塊匹配相似度閾值判斷，歸一化sad最小，并不代表就一定是正確匹配，比如光照變化、弱紋理等會造成誤匹配* 輸出：稀疏特征點視差圖/深度圖（亞像素精度）mvDepth 匹配結果 mvuRight*/// 為匹配結果預先分配內存，數據類型為float型// mvuRight存儲右圖匹配點索引// mvDepth存儲特征點的深度信息mvuRight = vector<float>(N,-1.0f);mvDepth = vector<float>(N,-1.0f);// orb特征相似度閾值  -> mean ～= (max  + min) / 2const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;// 金字塔底層（0層）圖像高 nRowsconst int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;// 二維vector存儲每一行的orb特征點的列坐標的索引，為什么是vector，因為每一行的特征點有可能不一樣，例如// vRowIndices[0] = [1，2，5，8, 11]   第1行有5個特征點,他們的列號（即x坐標）分別是1,2,5,8,11// vRowIndices[1] = [2，6，7，9, 13, 17, 20]  第2行有7個特征點.etcvector<vector<size_t> > vRowIndices(nRows, vector<size_t>());for(int i=0; i<nRows; i++) vRowIndices[i].reserve(200);// 右圖特征點數量，N表示數量 r表示右圖，且不能被修改const int Nr = mvKeysRight.size();// Step 1. 行特征點統計。 考慮用圖像金字塔尺度作為偏移，左圖中對應右圖的一個特征點可能存在于多行，而非唯一的一行for(int iR = 0; iR < Nr; iR++) {// 獲取特征點ir的y坐標，即行號const cv::KeyPoint &kp = mvKeysRight[iR];const float &kpY = kp.pt.y;// 計算特征點ir在行方向上，可能的偏移范圍r，即可能的行號為[kpY + r, kpY -r]// 2 表示在全尺寸(scale = 1)的情況下，假設有2個像素的偏移，隨著尺度變化，r也跟著變化const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave];const int maxr = ceil(kpY + r);const int minr = floor(kpY - r);// 將特征點ir保證在可能的行號中for(int yi=minr;yi<=maxr;yi++)vRowIndices[yi].push_back(iR);}// 下面是 粗匹配 + 精匹配的過程// 對于立體矯正后的兩張圖，在列方向(x)存在最大視差maxd和最小視差mind// 也即是左圖中任何一點p，在右圖上的匹配點的范圍為應該是[p - maxd, p - mind], 而不需要遍歷每一行所有的像素// maxd = baseline * length_focal / minZ// mind = baseline * length_focal / maxZconst float minZ = mb;const float minD = 0;			// 最小視差為0，對應無窮遠 const float maxD = mbf/minZ;    // 最大視差對應的距離是相機的基線// 保存sad塊匹配相似度和左圖特征點索引vector<pair<int, int> > vDistIdx;vDistIdx.reserve(N);// 為左圖每一個特征點il，在右圖搜索最相似的特征點irfor(int iL=0; iL<N; iL++) {const cv::KeyPoint &kpL = mvKeys[iL];const int &levelL = kpL.octave;const float &vL = kpL.pt.y;const float &uL = kpL.pt.x;// 獲取左圖特征點il所在行，以及在右圖對應行中可能的匹配點const vector<size_t> &vCandidates = vRowIndices[vL];if(vCandidates.empty()) continue;// 計算理論上的最佳搜索范圍const float minU = uL-maxD;const float maxU = uL-minD;// 最大搜索范圍小于0，說明無匹配點if(maxU<0) continue;// 初始化最佳相似度，用最大相似度，以及最佳匹配點索引int bestDist = ORBmatcher::TH_HIGH;size_t bestIdxR = 0;const cv::Mat &dL = mDescriptors.row(iL);// Step 2. 粗配準。左圖特征點il與右圖中的可能的匹配點進行逐個比較,得到最相似匹配點的描述子距離和索引for(size_t iC=0; iC<vCandidates.size(); iC++) {const size_t iR = vCandidates[iC];const cv::KeyPoint &kpR = mvKeysRight[iR];// 左圖特征點il與待匹配點ic的空間尺度差超過2，放棄if(kpR.octave<levelL-1 || kpR.octave>levelL+1)continue;// 使用列坐標(x)進行匹配，和stereomatch一樣const float &uR = kpR.pt.x;// 超出理論搜索范圍[minU, maxU]，可能是誤匹配，放棄if(uR >= minU && uR <= maxU) {// 計算匹配點il和待匹配點ic的相似度distconst cv::Mat &dR = mDescriptorsRight.row(iR);const int dist = ORBmatcher::DescriptorDistance(dL,dR);//統計最小相似度及其對應的列坐標(x)if( dist<bestDist ) {bestDist = dist;bestIdxR = iR;}}}// Step 3. 圖像塊滑動窗口用SAD(Sum of absolute differences，差的絕對和)實現精確匹配. if(bestDist<thOrbDist) {// 如果剛才匹配過程中的最佳描述子距離小于給定的閾值// 計算右圖特征點x坐標和對應的金字塔尺度const float uR0 = mvKeysRight[bestIdxR].pt.x;const float scaleFactor = mvInvScaleFactors[kpL.octave];// 尺度縮放后的左右圖特征點坐標const float scaleduL = round(kpL.pt.x*scaleFactor);			const float scaledvL = round(kpL.pt.y*scaleFactor);const float scaleduR0 = round(uR0*scaleFactor);// 滑動窗口搜索, 類似模版卷積或濾波// w表示sad相似度的窗口半徑const int w = 5;// 提取左圖中，以特征點(scaleduL,scaledvL)為中心, 半徑為w的圖像塊patchcv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);IL.convertTo(IL,CV_32F);// 圖像塊均值歸一化，降低亮度變化對相似度計算的影響IL = IL - IL.at<float>(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F);//初始化最佳相似度int bestDist = INT_MAX;// 通過滑動窗口搜索優化，得到的列坐標偏移量int bestincR = 0;//滑動窗口的滑動范圍為（-L, L）const int L = 5;// 初始化存儲圖像塊相似度vector<float> vDists;vDists.resize(2*L+1); // 計算滑動窗口滑動范圍的邊界，因為是塊匹配，還要算上圖像塊的尺寸// 列方向起點 iniu = r0 - 最大窗口滑動范圍 - 圖像塊尺寸// 列方向終點 eniu = r0 + 最大窗口滑動范圍 + 圖像塊尺寸 + 1// 此次 + 1 和下面的提取圖像塊是列坐標+1是一樣的，保證提取的圖像塊的寬是2 * w + 1// ! 源碼： const float iniu = scaleduR0+L-w; 錯誤// scaleduR0：右圖特征點x坐標const float iniu = scaleduR0-L-w;const float endu = scaleduR0+L+w+1;// 判斷搜索是否越界if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)continue;// 在搜索范圍內從左到右滑動，并計算圖像塊相似度for(int incR=-L; incR<=+L; incR++) {// 提取右圖中，以特征點(scaleduL,scaledvL)為中心, 半徑為w的圖像快patchcv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);IR.convertTo(IR,CV_32F);// 圖像塊均值歸一化，降低亮度變化對相似度計算的影響IR = IR - IR.at<float>(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);// sad 計算，值越小越相似float dist = cv::norm(IL,IR,cv::NORM_L1);// 統計最小sad和偏移量if(dist<bestDist) {bestDist = dist;bestincR = incR;}//L+incR 為refine后的匹配點列坐標(x)vDists[L+incR] = dist; 	}// 搜索窗口越界判斷if(bestincR==-L || bestincR==L)continue;// Step 4. 亞像素插值, 使用最佳匹配點及其左右相鄰點構成拋物線來得到最小sad的亞像素坐標// 使用3點擬合拋物線的方式，用極小值代替之前計算的最優是差值//    \                 / <- 由視差為14，15，16的相似度擬合的拋物線//      .             .(16)//         .14     .(15) <- int/uchar最佳視差值//              . //           （14.5）<- 真實的視差值//   deltaR = 15.5 - 16 = -0.5// 公式參考opencv sgbm源碼中的亞像素插值公式// 或論文<<On Building an Accurate Stereo Matching System on Graphics Hardware>> 公式7const float dist1 = vDists[L+bestincR-1];	const float dist2 = vDists[L+bestincR];const float dist3 = vDists[L+bestincR+1];const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2));// 亞像素精度的修正量應該是在[-1,1]之間，否則就是誤匹配if(deltaR<-1 || deltaR>1)continue;// 根據亞像素精度偏移量delta調整最佳匹配索引float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);float disparity = (uL-bestuR);if(disparity>=minD && disparity<maxD) {// 如果存在負視差，則約束為0.01if( disparity <=0 ) {disparity=0.01;bestuR = uL-0.01;}// 根據視差值計算深度信息// 保存最相似點的列坐標(x)信息// 保存歸一化sad最小相似度// Step 5. 最優視差值/深度選擇.mvDepth[iL]=mbf/disparity;mvuRight[iL] = bestuR;vDistIdx.push_back(pair<int,int>(bestDist,iL));}   }}// Step 6. 刪除離群點(outliers)// 塊匹配相似度閾值判斷，歸一化sad最小，并不代表就一定是匹配的，比如光照變化、弱紋理、無紋理等同樣會造成誤匹配// 誤匹配判斷條件  norm_sad > 1.5 * 1.4 * mediansort(vDistIdx.begin(),vDistIdx.end());const float median = vDistIdx[vDistIdx.size()/2].first;const float thDist = 1.5f*1.4f*median;for(int i=vDistIdx.size()-1;i>=0;i--) {if(vDistIdx[i].first<thDist)break;else {// 誤匹配點置為-1，和初始化時保持一直，作為error codemvuRight[vDistIdx[i].second]=-1;mvDepth[vDistIdx[i].second]=-1;}}
}

4.6 RBGD特征點的處理: 根據深度信息構造虛擬右目圖像:?ComputeStereoFromRGBD()

對于RGB特征點,根據深度信息構造虛擬右目圖像?

//計算RGBD圖像的立體深度信息
void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)	//參數是深度圖像
{/** 主要步驟如下:.對于彩色圖像中的每一個特征點:<ul>  */// mvDepth直接由depth圖像讀取`//這里是初始化這兩個存儲“右圖”匹配特征點橫坐標和存儲特征點深度值的vectormvuRight = vector<float>(N,-1);mvDepth = vector<float>(N,-1);//開始遍歷彩色圖像中的所有特征點for(int i=0; i<N; i++){/** <li> 從<b>未矯正的特征點</b>提供的坐標來讀取深度圖像拿到這個點的深度數據 </li> *///獲取校正前和校正后的特征點const cv::KeyPoint &kp = mvKeys[i];const cv::KeyPoint &kpU = mvKeysUn[i];//獲取其橫縱坐標，注意 NOTICE 是校正前的特征點的const float &v = kp.pt.y;const float &u = kp.pt.x;//從深度圖像中獲取這個特征點對應的深度點//NOTE 從這里看對深度圖像進行去畸變處理是沒有必要的,我們依舊可以直接通過未矯正的特征點的坐標來直接拿到深度數據const float d = imDepth.at<float>(v,u);///** <li> 如果獲取到的深度點合法(d>0), 那么就保存這個特征點的深度,并且計算出等效的\在假想的右圖中該特征點所匹配的特征點的橫坐標 </li>* \n 這個橫坐標的計算是 x-mbf/d* \n 其中的x使用的是<b>矯正后的</b>特征點的圖像坐標*/if(d>0){//那么就保存這個點的深度mvDepth[i] = d;//根據這個點的深度計算出等效的、在假想的右圖中的該特征點的橫坐標//TODO 話說為什么要計算這個嘞，計算出來之后有什么用?可能是為了保持計算一致mvuRight[i] = kpU.pt.x-mbf/d;}//如果獲取到的深度點合法}//開始遍歷彩色圖像中的所有特征點/** </ul> */
}

4.7 畸變矯正:?UndistortKeyPoints()

實際上,畸變矯正只對單目和RGBD相機輸入圖像有效,雙目相機的畸變矯正參數均為0,因為雙目相機數據集在發布之前預先做了雙目矯正.

• RGBD相機輸入配置文件TUM1.yaml

• Camera.k1: 0.262383
  Camera.k2: -0.953104
  Camera.p1: -0.005358
  Camera.p2: 0.002628
  Camera.k3: 1.163314
  ?
  #....
  

• 雙目相機輸入配置文件EuRoC.yaml

  
  Camera.k1: 0.0
  Camera.k2: 0.0
  Camera.p1: 0.0
  Camera.p2: 0.0
  ?
  # ...
  

• 單目相機輸入配置文件TUM1.yaml

  %YAML:1.0#--------------------------------------------------------------------------------------------
  # Camera Parameters. Adjust them!
  #--------------------------------------------------------------------------------------------# Camera calibration and distortion parameters (OpenCV) 
  Camera.fx: 517.306408
  Camera.fy: 516.469215
  Camera.cx: 318.643040
  Camera.cy: 255.313989Camera.k1: 0.262383
  Camera.k2: -0.953104
  Camera.p1: -0.005358
  Camera.p2: 0.002628
  Camera.k3: 1.163314

  雙目矯正效果如下,雙目矯正將兩個相機的成像平面矯正到同一平面上.雙目矯正之后兩個相機的極線相互平行,極點在無窮遠處,這也是我們在函數ComputeStereoMatches()中做極線搜索的理論基礎.

  ?UndistortKeyPoints()函數和ComputeImageBounds()內調用了cv::undistortPoints()函數對特征點進行畸變矯正

• void Frame::UndistortKeyPoints()
  {// Step 1 如果第一個畸變參數為0，不需要矯正。第一個畸變參數k1是最重要的，一般不為0，為0的話，說明畸變參數都是0//變量mDistCoef中存儲了opencv指定格式的去畸變參數，格式為：(k1,k2,p1,p2,k3)if(mDistCoef.at<float>(0)==0.0){mvKeysUn=mvKeys;return;}// Step 2 如果畸變參數不為0，用OpenCV函數進行畸變矯正// Fill matrix with points// N為提取的特征點數量，為滿足OpenCV函數輸入要求，將N個特征點保存在N*2的矩陣中cv::Mat mat(N,2,CV_32F);//遍歷每個特征點，并將它們的坐標保存到矩陣中for(int i=0; i<N; i++){//然后將這個特征點的橫縱坐標分別保存mat.at<float>(i,0)=mvKeys[i].pt.x;mat.at<float>(i,1)=mvKeys[i].pt.y;}// Undistort points// 函數reshape(int cn,int rows=0) 其中cn為更改后的通道數，rows=0表示這個行將保持原來的參數不變//為了能夠直接調用opencv的函數來去畸變，需要先將矩陣調整為2通道（對應坐標x,y） mat=mat.reshape(2);cv::undistortPoints(	mat,				//輸入的特征點坐標mat,				//輸出的校正后的特征點坐標覆蓋原矩陣mK,					//相機的內參數矩陣mDistCoef,			//相機畸變參數矩陣cv::Mat(),			//一個空矩陣，對應為函數原型中的RmK); 				//新內參數矩陣，對應為函數原型中的P//調整回只有一個通道，回歸我們正常的處理方式mat=mat.reshape(1);// Fill undistorted keypoint vector// Step 存儲校正后的特征點mvKeysUn.resize(N);//遍歷每一個特征點for(int i=0; i<N; i++){//根據索引獲取這個特征點//注意之所以這樣做而不是直接重新聲明一個特征點對象的目的是，能夠得到源特征點對象的其他屬性cv::KeyPoint kp = mvKeys[i];//讀取校正后的坐標并覆蓋老坐標kp.pt.x=mat.at<float>(i,0);kp.pt.y=mat.at<float>(i,1);mvKeysUn[i]=kp;}
  }/*** @brief 計算去畸變圖像的邊界* * @param[in] imLeft            需要計算邊界的圖像*/
  void Frame::ComputeImageBounds(const cv::Mat &imLeft)	
  {// 如果畸變參數不為0，用OpenCV函數進行畸變矯正if(mDistCoef.at<float>(0)!=0.0){// 保存矯正前的圖像四個邊界點坐標： (0,0) (cols,0) (0,rows) (cols,rows)cv::Mat mat(4,2,CV_32F);mat.at<float>(0,0)=0.0;         //左上mat.at<float>(0,1)=0.0;mat.at<float>(1,0)=imLeft.cols; //右上mat.at<float>(1,1)=0.0;mat.at<float>(2,0)=0.0;         //左下mat.at<float>(2,1)=imLeft.rows;mat.at<float>(3,0)=imLeft.cols; //右下mat.at<float>(3,1)=imLeft.rows;// Undistort corners// 和前面校正特征點一樣的操作，將這幾個邊界點作為輸入進行校正mat=mat.reshape(2);cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);mat=mat.reshape(1);//校正后的四個邊界點已經不能夠圍成一個嚴格的矩形，因此在這個四邊形的外側加邊框作為坐標的邊界mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));//左上和左下橫坐標最小的mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));//右上和右下橫坐標最大的mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));//左上和右上縱坐標最小的mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));//左下和右下縱坐標最小的}else{// 如果畸變參數為0，就直接獲得圖像邊界mnMinX = 0.0f;mnMaxX = imLeft.cols;mnMinY = 0.0f;mnMaxY = imLeft.rows;}
  }
  

  4.8 特征點分配:?AssignFeaturesToGrid()

• 在對特征點進行預處理后,將特征點分配到48行64列的網格中以加速匹配

  成員函數/變量	訪問控制	意義
  FRAME_GRID_ROWS=48?FRAME_GRID_COLS=64	#DEFINE宏	網格行數/列數
  float mfGridElementWidthInv?float mfGridElementHeightInv	public static?public static	每個網格的寬度/高度
  std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]	public	每個網格內特征點編號列表
  void AssignFeaturesToGrid()	private	將特征點分配到網格中
  vector GetFeaturesInArea(float &x, float &y, float &r, int minLevel, int maxLevel)	public	獲取半徑為r的圓域內的特征點編號列表

  成員變量std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]是一個二維數組,數組中每個元素是對應網格的所有特征點索引列表.

  static成員變量mfGridElementWidthInv、mfGridElementHeightInv表示網格寬度/高度,它們在第一次調用Frame構造函數時被計算賦值.

  Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth):mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),mpReferenceKF(static_cast<KeyFrame*>(NULL))
  {// Step 1 幀的ID 自增mnId=nNextId++;// Step 2 計算圖像金字塔的參數 //獲取圖像金字塔的層數mnScaleLevels = mpORBextractorLeft->GetLevels();//這個是獲得層與層之前的縮放比mfScaleFactor = mpORBextractorLeft->GetScaleFactor();//計算上面縮放比的對數, NOTICE log=自然對數，log10=才是以10為基底的對數 mfLogScaleFactor = log(mfScaleFactor);//獲取每層圖像的縮放因子mvScaleFactors = mpORBextractorLeft->GetScaleFactors();//同樣獲取每層圖像縮放因子的倒數mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();//高斯模糊的時候，使用的方差mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();//獲取sigma^2的倒數mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();// ORB extraction// Step 3 對左目右目圖像提取ORB特征點, 第一個參數0-左圖， 1-右圖。為加速計算，同時開了兩個線程計算thread threadLeft(&Frame::ExtractORB,		//該線程的主函數this,						//當前幀對象的對象指針0,						//表示是左圖圖像imLeft);					//圖像數據//對右目圖像提取ORB特征，參數含義同上thread threadRight(&Frame::ExtractORB,this,1,imRight);//等待兩張圖像特征點提取過程完成threadLeft.join();threadRight.join();//mvKeys中保存的是左圖像中的特征點，這里是獲取左側圖像中特征點的個數N = mvKeys.size();//如果左圖像中沒有成功提取到特征點那么就返回，也意味這這一幀的圖像無法使用if(mvKeys.empty())return;// Step 4 用OpenCV的矯正函數、內參對提取到的特征點進行矯正// 實際上由于雙目輸入的圖像已經預先經過矯正,所以實際上并沒有對特征點進行任何處理操作UndistortKeyPoints();// Step 5 計算雙目間特征點的匹配，只有匹配成功的特征點會計算其深度,深度存放在 mvDepth // mvuRight中存儲的應該是左圖像中的點所匹配的在右圖像中的點的橫坐標（縱坐標相同）ComputeStereoMatches();// 初始化本幀的地圖點mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));   // 記錄地圖點是否為外點，初始化均為外點falsemvbOutlier = vector<bool>(N,false);// This is done only for the first Frame (or after a change in the calibration)//  Step 5 計算去畸變后圖像邊界，將特征點分配到網格中。這個過程一般是在第一幀或者是相機標定參數發生變化之后進行if(mbInitialComputations){//計算去畸變后圖像的邊界ComputeImageBounds(imLeft);// 表示一個圖像像素相當于多少個圖像網格列（寬）mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);// 表示一個圖像像素相當于多少個圖像網格行（高）mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);//給類的靜態成員變量復制fx = K.at<float>(0,0);fy = K.at<float>(1,1);cx = K.at<float>(0,2);cy = K.at<float>(1,2);// 猜測是因為這種除法計算需要的時間略長，所以這里直接存儲了這個中間計算結果invfx = 1.0f/fx;invfy = 1.0f/fy;//特殊的初始化過程完成，標志復位mbInitialComputations=false;}// 雙目相機基線長度mb = mbf/fx;// 將特征點分配到圖像網格中 AssignFeaturesToGrid();    
  }

  函數AssignFeaturesToGrid()將特征點分配到網格中

  void Frame::AssignFeaturesToGrid()
  {// Step 1  給存儲特征點的網格數組 Frame::mGrid 預分配空間// ? 這里0.5 是為什么？節省空間？// FRAME_GRID_COLS = 64，FRAME_GRID_ROWS=48int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);//開始對mGrid這個二維數組中的每一個vector元素遍歷并預分配空間for(unsigned int i=0; i<FRAME_GRID_COLS;i++)for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)mGrid[i][j].reserve(nReserve);// Step 2 遍歷每個特征點，將每個特征點在mvKeysUn中的索引值放到對應的網格mGrid中for(int i=0;i<N;i++){//從類的成員變量中獲取已經去畸變后的特征點const cv::KeyPoint &kp = mvKeysUn[i];//存儲某個特征點所在網格的網格坐標，nGridPosX范圍：[0,FRAME_GRID_COLS], nGridPosY范圍：[0,FRAME_GRID_ROWS]int nGridPosX, nGridPosY;// 計算某個特征點所在網格的網格坐標，如果找到特征點所在的網格坐標，記錄在nGridPosX,nGridPosY里，返回true，沒找到返回falseif(PosInGrid(kp,nGridPosX,nGridPosY))//如果找到特征點所在網格坐標，將這個特征點的索引添加到對應網格的數組mGrid中mGrid[nGridPosX][nGridPosY].push_back(i);}
  }

  函數vector GetFeaturesInArea(float &x, float &y, float &r, int minLevel, int maxLevel)獲取點(y,x)周圍半徑為r的圓域內所有特征點編號.

4.9 構造函數:?Frame()?

Frame()構造函數依次進行上面介紹的步驟:

/*** @brief 為RGBD相機準備的幀構造函數* * @param[in] imGray        對RGB圖像灰度化之后得到的灰度圖像* @param[in] imDepth       深度圖像* @param[in] timeStamp     時間戳* @param[in] extractor     特征點提取器句柄* @param[in] voc           ORB特征點詞典的句柄* @param[in] K             相機的內參數矩陣* @param[in] distCoef      相機的去畸變參數* @param[in] bf            baseline*bf* @param[in] thDepth       遠點和近點的深度區分閾值*/
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth):mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{// Step 1 幀的ID 自增mnId=nNextId++;// Step 2 計算圖像金字塔的參數 //獲取圖像金字塔的層數mnScaleLevels = mpORBextractorLeft->GetLevels();//獲取每層的縮放因子mfScaleFactor = mpORBextractorLeft->GetScaleFactor();    //計算每層縮放因子的自然對數mfLogScaleFactor = log(mfScaleFactor);//獲取各層圖像的縮放因子mvScaleFactors = mpORBextractorLeft->GetScaleFactors();//獲取各層圖像的縮放因子的倒數mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();//TODO 也是獲取這個不知道有什么實際含義的sigma^2mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();//計算上面獲取的sigma^2的倒數mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();/** 3. 提取彩色圖像(其實現在已經灰度化成為灰度圖像了)的特征點 \n Frame::ExtractORB() */// ORB extraction// Step 3 對圖像進行提取特征點, 第一個參數0-左圖， 1-右圖ExtractORB(0,imGray);//獲取特征點的個數N = mvKeys.size();//如果這一幀沒有能夠提取出特征點，那么就直接返回了if(mvKeys.empty())return;// Step 4 用OpenCV的矯正函數、內參對提取到的特征點進行矯正UndistortKeyPoints();// Step 5 獲取圖像的深度，并且根據這個深度推算其右圖中匹配的特征點的視差ComputeStereoFromRGBD(imDepth);// 初始化本幀的地圖點mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));// 記錄地圖點是否為外點，初始化均為外點falsemvbOutlier = vector<bool>(N,false);// This is done only for the first Frame (or after a change in the calibration)//  Step 5 計算去畸變后圖像邊界，將特征點分配到網格中。這個過程一般是在第一幀或者是相機標定參數發生變化之后進行if(mbInitialComputations){//計算去畸變后圖像的邊界ComputeImageBounds(imGray);// 表示一個圖像像素相當于多少個圖像網格列（寬）mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);// 表示一個圖像像素相當于多少個圖像網格行（高）mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);//給類的靜態成員變量復制fx = K.at<float>(0,0);fy = K.at<float>(1,1);cx = K.at<float>(0,2);cy = K.at<float>(1,2);// 猜測是因為這種除法計算需要的時間略長，所以這里直接存儲了這個中間計算結果invfx = 1.0f/fx;invfy = 1.0f/fy;//特殊的初始化過程完成，標志復位mbInitialComputations=false;}// 計算假想的基線長度 baseline= mbf/fx// 后面要對從RGBD相機輸入的特征點,結合相機基線長度,焦距,以及點的深度等信息來計算其在假想的"右側圖像"上的匹配點mb = mbf/fx;// 將特征點分配到圖像網格中 AssignFeaturesToGrid();
}

4.10?Frame類的用途

?Tracking類有兩個Frame類型的成員變量

?Tracking線程每收到一幀圖像,就調用函數Tracking::GrabImageMonocular()、Tracking::GrabImageStereo()或Tracking::GrabImageRGBD()創建一個Frame對象,賦值給mCurrentFrame

/*** @brief * 輸入左目RGB或RGBA圖像，輸出世界坐標系到該幀相機坐標系的變換矩陣* * @param[in] im 單目圖像* @param[in] timestamp 時間戳* @return cv::Mat * * Step 1 ：將彩色圖像轉為灰度圖像* Step 2 ：構造Frame* Step 3 ：跟蹤*/
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im,const double &timestamp)
{mImGray = im;// Step 1 ：將彩色圖像轉為灰度圖像//若圖片是3、4通道的，還需要轉化成灰度圖if(mImGray.channels()==3){if(mbRGB)cvtColor(mImGray,mImGray,CV_RGB2GRAY);elsecvtColor(mImGray,mImGray,CV_BGR2GRAY);}else if(mImGray.channels()==4){if(mbRGB)cvtColor(mImGray,mImGray,CV_RGBA2GRAY);elsecvtColor(mImGray,mImGray,CV_BGRA2GRAY);}// Step 2 ：構造Frame//判斷該幀是不是初始化if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) //沒有成功初始化的前一個狀態就是NO_IMAGES_YETmCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,      //初始化ORB特征點提取器會提取2倍的指定特征點數目mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);elsemCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,     //正常運行的時的ORB特征點提取器，提取指定數目特征點mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);// Step 3 ：跟蹤Track();//返回當前幀的位姿return mCurrentFrame.mTcw.clone();
}

Track()函數跟蹤結束后,會將mCurrentFrame賦值給mLastFrame?

void Tracking::Track() {// 進行跟蹤// ...// 將當前幀記錄為上一幀mLastFrame = Frame(mCurrentFrame);// ...
}

?除了少數被選為KeyFrame的幀以外,大部分Frame對象的作用僅在于Tracking線程內追蹤當前幀位姿,不會對LocalMapping線程和LoopClosing線程產生任何影響,在mLastFrame和mCurrentFrame更新之后就被系統銷毀了.

5. ORB-SLAM2代碼詳解05_關鍵幀KeyFrame（對應文件KeyFrame.cc,KeyFrame.h)

5.1.1 共視圖:?mConnectedKeyFrameWeights

能看到同一地圖點的兩關鍵幀之間存在共視關系,共視地圖點的數量被稱為權重.

共視圖結構由3個成員變量維護:

mConnectedKeyFrameWeights是一個std::map,無序地保存當前關鍵幀的共視關鍵幀及權重.
mvpOrderedConnectedKeyFrames和mvOrderedWeights按權重降序分別保存當前關鍵幀的共視關鍵幀列表和權重列表.

5.1.2 基于對地圖點的觀測重新構造共視圖:?UpdateConnections()

這3個變量由函數KeyFrame::UpdateConnections()進行初始化和維護,基于當前關鍵幀看到的地圖點信息重新生成共視關鍵幀.

void KeyFrame::UpdateConnections()
{// 關鍵幀-權重，權重為其它關鍵幀與當前關鍵幀共視地圖點的個數，也稱為共視程度map<KeyFrame*,int> KFcounter; vector<MapPoint*> vpMP;{// 獲得該關鍵幀的所有地圖點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// Step 1 通過地圖點被關鍵幀觀測來間接統計關鍵幀之間的共視程度// 統計每一個地圖點都有多少關鍵幀與當前關鍵幀存在共視關系，統計結果放在KFcounterfor(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++){MapPoint* pMP = *vit;if(!pMP)continue;if(pMP->isBad())continue;// 對于每一個地圖點，observations記錄了可以觀測到該地圖點的所有關鍵幀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 counterint 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;}// 建立共視關系至少需要大于等于th個共視地圖點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;// 更新當前幀與其它關鍵幀的連接權重// ?bug 這里直接賦值，會把小于閾值的共視關系也放入mConnectedKeyFrameWeights，會增加計算量// ?但后續主要用mvpOrderedConnectedKeyFrames來取共視幀，對結果沒影響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;}}
}

只要關鍵幀與地圖點間的連接關系發生變化(包括關鍵幀創建和地圖點重新匹配關鍵幀特征點),函數KeyFrame::UpdateConnections()就會被調用.具體來說,函數KeyFrame::UpdateConnections()的調用時機包括:

Tracking線程中初始化函數Tracking::StereoInitialization()或Tracking::MonocularInitialization()函數創建關鍵幀后會調用KeyFrame::UpdateConnections()初始化共視圖信息.
LocalMapping線程接受到新關鍵幀時會調用函數LocalMapping::ProcessNewKeyFrame()處理跟蹤過程中加入的地圖點,之后會調用KeyFrame::UpdateConnections()初始化共視圖信息.(實際上這里處理的是Tracking線程中函數Tracking::CreateNewKeyFrame()創建的關鍵幀)
LocalMapping線程處理完畢緩沖隊列內所有關鍵幀后會調用LocalMapping::SearchInNeighbors()融合當前關鍵幀和共視關鍵幀間的重復地圖點,之后會調用KeyFrame::UpdateConnections()更新共視圖信息.
LoopClosing線程閉環矯正函數LoopClosing::CorrectLoop()會多次調用KeyFrame::UpdateConnections()更新共視圖信息.

函數AddConnection(KeyFrame* pKF, const int &weight)和EraseConnection(KeyFrame* pKF)先對變量mConnectedKeyFrameWeights進行修改,再調用函數UpdateBestCovisibles()修改變量mvpOrderedConnectedKeyFrames和mvOrderedWeights.

這3個函數都只在函數KeyFrame::UpdateConnections()內部被調用了,應該設為私有成員函數.

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) {// step1. 修改變量mConnectedKeyFrameWeights{unique_lock<mutex> lock(mMutexConnections);
?if (!mConnectedKeyFrameWeights.count(pKF) || mConnectedKeyFrameWeights[pKF] != weight)mConnectedKeyFrameWeights[pKF] = weight;elsereturn;}// step2. 調用函數UpdateBestCovisibles()修改變量mvpOrderedConnectedKeyFrames和mvOrderedWeightsUpdateBestCovisibles();
}
?
?
void KeyFrame::EraseConnection(KeyFrame *pKF) {// step1. 修改變量mConnectedKeyFrameWeightsbool bUpdate = false;{unique_lock<mutex> lock(mMutexConnections);if (mConnectedKeyFrameWeights.count(pKF)) {mConnectedKeyFrameWeights.erase(pKF);bUpdate = true;}}
?// step2. 調用函數UpdateBestCovisibles()修改變量mvpOrderedConnectedKeyFrames和mvOrderedWeightsif (bUpdate)UpdateBestCovisibles();
}
?
void KeyFrame::UpdateBestCovisibles() {    unique_lock<mutex> lock(mMutexConnections);// 取出所有關鍵幀進行排序,排序結果存入變量mvpOrderedConnectedKeyFrames和mvOrderedWeights中vector<pair<int, KeyFrame *> > vPairs;vPairs.reserve(mConnectedKeyFrameWeights.size());for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++)vPairs.push_back(make_pair(mit->second, mit->first));
?sort(vPairs.begin(), vPairs.end());list<KeyFrame *> lKFs; list<int> lWs; for (size_t i = 0, iend = vPairs.size(); i < iend; i++) {lKFs.push_front(vPairs[i].second);lWs.push_front(vPairs[i].first);}
?mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}

5.2 生成樹: mpParent、mspChildrens
生成樹是一種稀疏連接,以最小的邊數保存圖中所有節點.對于含有N個節點的圖,只需構造一個N-1條邊的最小生成樹就可以將所有節點連接起來.

下圖表示含有一個10個節點,20條邊的稠密圖;粗黑線代表其最小生成樹,只需9條邊即可將所有節點連接起來.

?在ORB-SLAM2中,保存所有關鍵幀構成的最小生成樹(優先選擇權重大的邊作為生成樹的邊),在回環閉合時只需對最小生成樹做BA優化就能以最小代價優化所有關鍵幀和地圖點的位姿,相比于優化共視圖大大減少了計算量.(實際上并沒有對最小生成樹做BA優化,而是對包含生成樹的本質圖做BA優化)

生成樹結構由成員變量mpParent和mspChildrens維護.我們主要關注生成樹結構發生改變的時機.

關鍵幀增加到生成樹中的時機:

成功創建關鍵幀之后會調用函數KeyFrame::UpdateConnections(),該函數第一次被調用時會將該新關鍵幀加入到生成樹中.

新關鍵幀的父關鍵幀會被設為其共視程度最高的共視關鍵幀.

void KeyFrame::UpdateConnections() {// 更新共視圖信息// ...// 更新關鍵幀信息: 對于第一次加入生成樹的關鍵幀,取共視程度最高的關鍵幀為父關鍵幀// 該操作會改變當前關鍵幀的成員變量mpParent和父關鍵幀的成員變量mspChildrensunique_lock<mutex> lockCon(mMutexConnections);if (mbFirstConnection && mnId != 0) {mpParent = mvpOrderedConnectedKeyFrames.front();mpParent->AddChild(this);mbFirstConnection = false;}
}

共視圖的改變(除了刪除關鍵幀以外)不會引發生成樹的改變.
只有當某個關鍵幀刪除時,與其相連的生成樹結構在會發生改變.(因為生成樹是個單線聯系的結構,沒有冗余,一旦某關鍵幀刪除了就得更新樹結構才能保證所有關鍵幀依舊相連).生成樹結構改變的方式類似于最小生成樹算法中的加邊法,見后文對函數setbadflag()的分析.
?

5.3 關鍵幀的刪除?

?與MapPoint類似,函數KeyFrame::SetBadFlag()對KeyFrame的刪除過程也采取先標記再清除的方式: 先將壞幀標記mBad置為true,再依次處理其各成員變量.

?5.4 參與回環檢測的關鍵幀具有不被刪除的特權:?mbNotErase

參與回環檢測的關鍵幀具有不被刪除的特權,該特權由成員變量mbNotErase存儲,創建KeyFrame對象時該成員變量默認被初始化為false.

若某關鍵幀參與了回環檢測,LoopClosing線程就會就調用函數KeyFrame::SetNotErase()將該關鍵幀的成員變量mbNotErase設為true,標記該關鍵幀暫時不要被刪除.

void KeyFrame::SetNotErase() {unique_lock<mutex> lock(mMutexConnections);mbNotErase = true;
}

在刪除函數SetBadFlag()起始先根據成員變量mbNotErase判斷當前KeyFrame是否具有豁免刪除的特權.若當前KeyFrame的mbNotErase為true,則函數SetBadFlag()不能刪除當前KeyFrame,但會將其成員變量mbToBeErased置為true.

/*** @brief 真正地執行刪除關鍵幀的操作* 需要刪除的是該關鍵幀和其他所有幀、地圖點之間的連接關系* * mbNotErase作用：表示要刪除該關鍵幀及其連接關系但是這個關鍵幀有可能正在回環檢測或者計算sim3操作，這時候雖然這個關鍵幀冗余，但是卻不能刪除，* 僅設置mbNotErase為true，這時候調用setbadflag函數時，不會將這個關鍵幀刪除，只會把mbTobeErase變成true，代表這個關鍵幀可以刪除但不到時候,先記下來以后處理。* 在閉環線程里調用 SetErase()會根據mbToBeErased 來刪除之前可以刪除還沒刪除的幀。*/
void KeyFrame::SetBadFlag()
{   // Step 1 首先處理一下刪除不了的特殊情況{unique_lock<mutex> lock(mMutexConnections);// 第0關鍵幀不允許被刪除if(mnId==0)return;else if(mbNotErase){// mbNotErase表示不應該刪除，于是把mbToBeErased置為true，假裝已經刪除，其實沒有刪除mbToBeErased = true;return;}}// Step 2 遍歷所有和當前關鍵幀相連的關鍵幀，刪除他們與當前關鍵幀的聯系for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)mit->first->EraseConnection(this); // 讓其它的關鍵幀刪除與自己的聯系// Step 3 遍歷每一個當前關鍵幀的地圖點，刪除每一個地圖點和當前關鍵幀的聯系for(size_t i=0; i<mvpMapPoints.size(); i++)if(mvpMapPoints[i])mvpMapPoints[i]->EraseObservation(this); {unique_lock<mutex> lock(mMutexConnections);unique_lock<mutex> lock1(mMutexFeatures);// 清空自己與其它關鍵幀之間的聯系mConnectedKeyFrameWeights.clear();mvpOrderedConnectedKeyFrames.clear();// Update Spanning Tree // Step 4 更新生成樹，主要是處理好父子關鍵幀，不然會造成整個關鍵幀維護的圖斷裂，或者混亂// 候選父關鍵幀set<KeyFrame*> sParentCandidates;// 將當前幀的父關鍵幀放入候選父關鍵幀sParentCandidates.insert(mpParent);// Assign at each iteration one children with a parent (the pair with highest covisibility weight)// Include that children as new parent candidate for the rest// 每迭代一次就為其中一個子關鍵幀尋找父關鍵幀（最高共視程度），找到父的子關鍵幀可以作為其他子關鍵幀的候選父關鍵幀while(!mspChildrens.empty()){bool bContinue = false;int max = -1;KeyFrame* pC;KeyFrame* pP;// Step 4.1 遍歷每一個子關鍵幀，讓它們更新它們指向的父關鍵幀for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++){KeyFrame* pKF = *sit;// 跳過無效的子關鍵幀if(pKF->isBad())    continue;// Check if a parent candidate is connected to the keyframe// Step 4.2 子關鍵幀遍歷每一個與它共視的關鍵幀    vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();for(size_t i=0, iend=vpConnected.size(); i<iend; i++){// sParentCandidates 中剛開始存的是這里子關鍵幀的“爺爺”，也是當前關鍵幀的候選父關鍵幀for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++){// Step 4.3 如果孩子和sParentCandidates中有共視，選擇共視最強的那個作為新的父if(vpConnected[i]->mnId == (*spcit)->mnId){int w = pKF->GetWeight(vpConnected[i]);// 尋找并更新權值最大的那個共視關系if(w>max){pC = pKF;                   //子關鍵幀pP = vpConnected[i];        //目前和子關鍵幀具有最大權值的關鍵幀（將來的父關鍵幀） max = w;                    //這個最大的權值bContinue = true;           //說明子節點找到了可以作為其新父關鍵幀的幀}}}}}// Step 4.4 如果在上面的過程中找到了新的父節點// 下面代碼應該放到遍歷子關鍵幀循環中?// 回答：不需要！這里while循環還沒退出，會使用更新的sParentCandidatesif(bContinue){// 因為父節點死了，并且子節點找到了新的父節點，就把它更新為自己的父節點pC->ChangeParent(pP);// 因為子節點找到了新的父節點并更新了父節點，那么該子節點升級，作為其它子節點的備選父節點sParentCandidates.insert(pC);// 該子節點處理完畢，刪掉mspChildrens.erase(pC);}elsebreak;}// If a children has no covisibility links with any parent candidate, assign to the original parent of this KF// Step 4.5 如果還有子節點沒有找到新的父節點if(!mspChildrens.empty())for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++){// 直接把父節點的父節點作為自己的父節點 即對于這些子節點來說,他們的新的父節點其實就是自己的爺爺節點(*sit)->ChangeParent(mpParent);}mpParent->EraseChild(this);// mTcp 表示原父關鍵幀到當前關鍵幀的位姿變換，在保存位姿的時候使用mTcp = Tcw*mpParent->GetPoseInverse();// 標記當前關鍵幀已經掛了mbBad = true;}  // 地圖和關鍵幀數據庫中刪除該關鍵幀mpMap->EraseKeyFrame(this);mpKeyFrameDB->erase(this);
}

成員變量mbToBeErased標記當前KeyFrame是否被豁免過刪除特權.LoopClosing線程不再需要某關鍵幀時,會調用函數KeyFrame::SetErase()剝奪該關鍵幀不被刪除的特權,將成員變量mbNotErase復位為false;同時檢查成員變量mbToBeErased,若mbToBeErased為true就會調用函數KeyFrame::SetBadFlag()刪除該關鍵幀.

/*** @brief 刪除當前的這個關鍵幀,表示不進行回環檢測過程;由回環檢測線程調用* */
void KeyFrame::SetErase()
{{unique_lock<mutex> lock(mMutexConnections);// 如果當前關鍵幀和其他的關鍵幀沒有形成回環關系,那么就刪吧if(mspLoopEdges.empty()){mbNotErase = false;}}// mbToBeErased：刪除之前記錄的想要刪但時機不合適沒有刪除的幀if(mbToBeErased){SetBadFlag();}
}

5.5 刪除關鍵幀時維護共視圖和生成樹

函數SetBadFlag()在刪除關鍵幀的時維護其共視圖和生成樹結構.共視圖結構的維護比較簡單,這里主要關心如何維護生成樹的結構.

當一個關鍵幀被刪除時,其父關鍵幀和所有子關鍵幀的生成樹信息也會受到影響,需要為其所有子關鍵幀尋找新的父關鍵幀,如果父關鍵幀找的不好的話,就會產生回環,導致生成樹就斷開.

被刪除關鍵幀的子關鍵幀所有可能的父關鍵幀包括其兄弟關鍵幀和其被刪除關鍵幀的父關鍵幀.以下圖為例,關鍵幀4可能的父關鍵幀包括關鍵幀3、5、6和7.

采用類似于最小生成樹算法中的加邊法重新構建生成樹結構: 每次循環取權重最高的候選邊建立父子連接關系,并將新加入生成樹的子節點到加入候選父節點集合sParentCandidates中.

void KeyFrame::SetBadFlag() {// step1. 特殊情況:豁免 第一幀 和 具有mbNotErase特權的幀{unique_lock<mutex> lock(mMutexConnections);
?if (mnId == 0)return;else if (mbNotErase) {mbToBeErased = true;return;}}
?// step2. 從共視關鍵幀的共視圖中刪除本關鍵幀for (auto mit : mConnectedKeyFrameWeights)mit.first->EraseConnection(this);
?// step3. 刪除當前關鍵幀中地圖點對本幀的觀測for (size_t i = 0; i < mvpMapPoints.size(); i++)if (mvpMapPoints[i])mvpMapPoints[i]->EraseObservation(this);
?{// step4. 刪除共視圖unique_lock<mutex> lock(mMutexConnections);unique_lock<mutex> lock1(mMutexFeatures);mConnectedKeyFrameWeights.clear();mvpOrderedConnectedKeyFrames.clear();
?// step5. 更新生成樹結構set<KeyFrame *> sParentCandidates;sParentCandidates.insert(mpParent);
?while (!mspChildrens.empty()) {bool bContinue = false;int max = -1;KeyFrame *pC;KeyFrame *pP;for (KeyFrame *pKF : mspChildrens) {if (pKF->isBad())continue;
?vector<KeyFrame *> vpConnected = pKF->GetVectorCovisibleKeyFrames();
?for (size_t i = 0, iend = vpConnected.size(); i < iend; i++) {for (set<KeyFrame *>::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end();spcit != spcend; spcit++) {if (vpConnected[i]->mnId == (*spcit)->mnId) {int w = pKF->GetWeight(vpConnected[i]);if (w > max) {pC = pKF;                   pP = vpConnected[i];        max = w;                    bContinue = true;           }}}}}
?if (bContinue) {pC->ChangeParent(pP);sParentCandidates.insert(pC);mspChildrens.erase(pC);} elsebreak;}
?if (!mspChildrens.empty())for (set<KeyFrame *>::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++) {(*sit)->ChangeParent(mpParent);}
?mpParent->EraseChild(this);mTcp = Tcw * mpParent->GetPoseInverse();// step6. 將當前關鍵幀的 mbBad 置為 truembBad = true;} // step7. 從地圖中刪除當前關鍵幀mpMap->EraseKeyFrame(this);mpKeyFrameDB->erase(this);
}

5.6 對地圖點的觀測
KeyFrame類除了像一般的Frame類那樣保存二維圖像特征點以外,還保存三維地圖點MapPoint信息.

關鍵幀觀測到的地圖點列表由成員變量mvpMapPoints保存,下面是一些對該成員變量進行增刪改查的成員函數,就是簡單的列表操作,沒什么值得說的地方.

?值得關心的是上述函數的調用時機,也就是說參考幀何時與地圖點發生關系:

關鍵幀增加對地圖點觀測的時機:

Tracking線程和LocalMapping線程創建新地圖點后,會馬上調用函數KeyFrame::AddMapPoint()添加當前關鍵幀對該地圖點的觀測.
LocalMapping線程處理完畢緩沖隊列內所有關鍵幀后會調用LocalMapping::SearchInNeighbors()融合當前關鍵幀和共視關鍵幀間的重復地圖點,其中調用函數ORBmatcher::Fuse()實現融合過程中會調用函數KeyFrame::AddMapPoint().
LoopClosing線程閉環矯正函數LoopClosing::CorrectLoop()將閉環關鍵幀與其匹配關鍵幀間的地圖進行融合,會調用函數KeyFrame::AddMapPoint().
關鍵幀替換和刪除對地圖點觀測的時機:

MapPoint刪除函數MapPoint::SetBadFlag()或替換函數MapPoint::Replace()會調用KeyFrame::EraseMapPointMatch()和KeyFrame::ReplaceMapPointMatch()刪除和替換關鍵針對地圖點的觀測.

LocalMapping線程調用進行局部BA優化的函數Optimizer::LocalBundleAdjustment()內部調用函數KeyFrame::EraseMapPointMatch()刪除對重投影誤差較大的地圖點的觀測.

5.7 回環檢測與本質圖

LoopClosing線程中回環矯正函數LoopClosing::CorrectLoop()在調用本質圖BA優化函數Optimizer::OptimizeEssentialGraph()之前會調用函數KeyFrame::AddLoopEdge(),在當前關鍵幀和其閉環匹配關鍵幀間添加回環關系.

在調用本質圖BA優化函數Optimizer::OptimizeEssentialGraph()中會調用函數KeyFrame::GetLoopEdges()將所有閉環關系加入到本質圖中進行優化.

共視圖比較稠密，本質圖比共視圖更稀疏，這是因為本質圖的作用是用在閉環矯正時，用相似變換來矯
正尺度漂移，把閉環誤差均攤在本質圖中。本質圖中節點也是所有關鍵幀，但是連接邊更少，只保留了
聯系緊密的邊來使得結果更精確。本質圖中包含：
1. 擴展樹連接關系
2. 形成閉環的連接關系，閉環后地圖點變動后新增加的連接關系
3. 共視關系非常好（至少100個共視地圖點）的連接關系

void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,const LoopClosing::KeyFrameAndPose &CorrectedSim3,const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)
{// Setup optimizer// Step 1：構造優化器g2o::SparseOptimizer optimizer;optimizer.setVerbose(false);g2o::BlockSolver_7_3::LinearSolverType * linearSolver =new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver);// 使用LM算法進行非線性迭代g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);// 第一次迭代的初始lambda值，如未指定會自動計算一個合適的值solver->setUserLambdaInit(1e-16);optimizer.setAlgorithm(solver);// 獲取當前地圖中的所有關鍵幀 和地圖點const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();// 最大關鍵幀id，用于添加頂點時使用const unsigned int nMaxKFid = pMap->GetMaxKFid();// 記錄所有優化前關鍵幀的位姿，優先使用在閉環時通過Sim3傳播調整過的Sim3位姿vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vScw(nMaxKFid+1);// 記錄所有關鍵幀經過本次本質圖優化過的位姿vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vCorrectedSwc(nMaxKFid+1);// 這個變量沒有用vector<g2o::VertexSim3Expmap*> vpVertices(nMaxKFid+1);// 兩個關鍵幀之間共視關系的權重的最小值const int minFeat = 100;// Set KeyFrame vertices// Step 2：將地圖中所有關鍵幀的位姿作為頂點添加到優化器// 盡可能使用經過Sim3調整的位姿// 遍歷全局地圖中的所有的關鍵幀for(size_t i=0, iend=vpKFs.size(); i<iend;i++){KeyFrame* pKF = vpKFs[i];if(pKF->isBad())continue;g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();// 關鍵幀在所有關鍵幀中的id，用來設置為頂點的idconst int nIDi = pKF->mnId;LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF);if(it!=CorrectedSim3.end()){// 如果該關鍵幀在閉環時通過Sim3傳播調整過，優先用調整后的Sim3位姿vScw[nIDi] = it->second;VSim3->setEstimate(it->second);}else{// 如果該關鍵幀在閉環時沒有通過Sim3傳播調整過，用跟蹤時的位姿，尺度為1Eigen::Matrix<double,3,3> Rcw = Converter::toMatrix3d(pKF->GetRotation());Eigen::Matrix<double,3,1> tcw = Converter::toVector3d(pKF->GetTranslation());g2o::Sim3 Siw(Rcw,tcw,1.0); vScw[nIDi] = Siw;VSim3->setEstimate(Siw);}// 閉環匹配上的幀不進行位姿優化（認為是準確的，作為基準）// 注意這里并沒有鎖住第0個關鍵幀，所以初始關鍵幀位姿也做了優化if(pKF==pLoopKF)VSim3->setFixed(true);VSim3->setId(nIDi);VSim3->setMarginalized(false);// 和當前系統的傳感器有關，如果是RGBD或者是雙目，那么就不需要優化sim3的縮放系數，保持為1即可VSim3->_fix_scale = bFixScale;// 添加頂點optimizer.addVertex(VSim3);// 優化前的位姿頂點，后面代碼中沒有使用vpVertices[nIDi]=VSim3;}// 保存由于閉環后優化sim3而出現的新的關鍵幀和關鍵幀之間的連接關系,其中id比較小的關鍵幀在前,id比較大的關鍵幀在后set<pair<long unsigned int,long unsigned int> > sInsertedEdges;// 單位矩陣const Eigen::Matrix<double,7,7> matLambda = Eigen::Matrix<double,7,7>::Identity();// Set Loop edges// Step 3：添加第1種邊：閉環時因為地圖點調整而出現的關鍵幀間的新連接關系for(map<KeyFrame *, set<KeyFrame *> >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++){KeyFrame* pKF = mit->first;const long unsigned int nIDi = pKF->mnId;// 和pKF 形成新連接關系的關鍵幀const set<KeyFrame*> &spConnections = mit->second;const g2o::Sim3 Siw = vScw[nIDi];const g2o::Sim3 Swi = Siw.inverse();// 對于當前關鍵幀nIDi而言，遍歷每一個新添加的關鍵幀nIDj鏈接關系for(set<KeyFrame*>::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++){const long unsigned int nIDj = (*sit)->mnId;// 同時滿足下面2個條件的跳過// 條件1：至少有一個不是pCurKF或pLoopKF// 條件2：共視程度太少(<100),不足以構成約束的邊if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId)   && pKF->GetWeight(*sit)<minFeat)       continue;// 通過上面考驗的幀有兩種情況：// 1、恰好是當前幀及其閉環幀 nIDi=pCurKF 并且nIDj=pLoopKF（此時忽略共視程度）// 2、任意兩對關鍵幀，共視程度大于100const g2o::Sim3 Sjw = vScw[nIDj];// 得到兩個位姿間的Sim3變換const g2o::Sim3 Sji = Sjw * Swi;g2o::EdgeSim3* e = new g2o::EdgeSim3();e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));// Sji內部是經過了Sim調整的觀測e->setMeasurement(Sji);// 信息矩陣是單位陣,說明這類新增加的邊對總誤差的貢獻也都是一樣大的e->information() = matLambda;optimizer.addEdge(e);// 保證id小的在前,大的在后sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj)));} }// Set normal edges// Step 4：添加跟蹤時形成的邊、閉環匹配成功形成的邊for(size_t i=0, iend=vpKFs.size(); i<iend; i++){KeyFrame* pKF = vpKFs[i];const int nIDi = pKF->mnId;g2o::Sim3 Swi;LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);if(iti!=NonCorrectedSim3.end())Swi = (iti->second).inverse();  //優先使用未經過Sim3傳播調整的位姿elseSwi = vScw[nIDi].inverse();     //沒找到才考慮已經經過Sim3傳播調整的位姿KeyFrame* pParentKF = pKF->GetParent();// Spanning tree edge// Step 4.1：添加第2種邊：生成樹的邊（有父關鍵幀）// 父關鍵幀就是和當前幀共視程度最高的關鍵幀if(pParentKF){// 父關鍵幀idint nIDj = pParentKF->mnId;g2o::Sim3 Sjw;LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);//優先使用未經過Sim3傳播調整的位姿if(itj!=NonCorrectedSim3.end())Sjw = itj->second;elseSjw = vScw[nIDj];// 計算父子關鍵幀之間的相對位姿g2o::Sim3 Sji = Sjw * Swi;g2o::EdgeSim3* e = new g2o::EdgeSim3();e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));// 希望父子關鍵幀之間的位姿差最小e->setMeasurement(Sji);// 所有元素的貢獻都一樣;每個誤差邊對總誤差的貢獻也都相同e->information() = matLambda;optimizer.addEdge(e);}// Loop edges// Step 4.2：添加第3種邊：當前幀與閉環匹配幀之間的連接關系(這里面也包括了當前遍歷到的這個關鍵幀之前曾經存在過的回環邊)// 獲取和當前關鍵幀形成閉環關系的關鍵幀const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();for(set<KeyFrame*>::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++){KeyFrame* pLKF = *sit;// 注意要比當前遍歷到的這個關鍵幀的id小,這個是為了避免重復添加if(pLKF->mnId<pKF->mnId){g2o::Sim3 Slw;LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);//優先使用未經過Sim3傳播調整的位姿if(itl!=NonCorrectedSim3.end())Slw = itl->second;elseSlw = vScw[pLKF->mnId];g2o::Sim3 Sli = Slw * Swi;g2o::EdgeSim3* el = new g2o::EdgeSim3();el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pLKF->mnId)));el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));// 根據兩個位姿頂點的位姿算出相對位姿作為邊el->setMeasurement(Sli);el->information() = matLambda;optimizer.addEdge(el);}}// Covisibility graph edges// Step 4.3：添加第4種邊：共視程度超過100的關鍵幀也作為邊進行優化// 取出和當前關鍵幀共視程度超過100的關鍵幀const vector<KeyFrame*> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat);for(vector<KeyFrame*>::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++){KeyFrame* pKFn = *vit;// 避免重復添加// 避免以下情況：最小生成樹中的父子關鍵幀關系,以及和當前遍歷到的關鍵幀構成了回環關系if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) {// 注意要比當前遍歷到的這個關鍵幀的id要小,這個是為了避免重復添加if(!pKFn->isBad() && pKFn->mnId<pKF->mnId){// 如果這條邊已經添加了，跳過if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId))))continue;g2o::Sim3 Snw;LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);// 優先未經過Sim3傳播調整的位姿if(itn!=NonCorrectedSim3.end())Snw = itn->second;elseSnw = vScw[pKFn->mnId];// 也是同樣計算相對位姿g2o::Sim3 Sni = Snw * Swi;g2o::EdgeSim3* en = new g2o::EdgeSim3();en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFn->mnId)));en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));en->setMeasurement(Sni);en->information() = matLambda;optimizer.addEdge(en);}} // 如果這個比較好的共視關系的約束之前沒有被重復添加過} // 遍歷所有于當前遍歷到的關鍵幀具有較好的共視關系的關鍵幀} // 添加跟蹤時形成的邊、閉環匹配成功形成的邊// Optimize!// Step 5：開始g2o優化，迭代20次optimizer.initializeOptimization();optimizer.optimize(20);// 更新地圖前，先上鎖，防止沖突unique_lock<mutex> lock(pMap->mMutexMapUpdate);// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]// Step 6：將優化后的位姿更新到關鍵幀中// 遍歷地圖中的所有關鍵幀for(size_t i=0;i<vpKFs.size();i++){KeyFrame* pKFi = vpKFs[i];const int nIDi = pKFi->mnId;g2o::VertexSim3Expmap* VSim3 = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(nIDi));g2o::Sim3 CorrectedSiw =  VSim3->estimate();vCorrectedSwc[nIDi]=CorrectedSiw.inverse();Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();Eigen::Vector3d eigt = CorrectedSiw.translation();double s = CorrectedSiw.scale();// 轉換成尺度為1的變換矩陣的形式eigt *=(1./s); //[R t/s;0 1]cv::Mat Tiw = Converter::toCvSE3(eigR,eigt);// 將更新的位姿寫入到關鍵幀中pKFi->SetPose(Tiw);}// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose// Step 7：步驟5和步驟6優化得到關鍵幀的位姿后，地圖點根據參考幀優化前后的相對關系調整自己的位置// 遍歷所有地圖點for(size_t i=0, iend=vpMPs.size(); i<iend; i++){MapPoint* pMP = vpMPs[i];if(pMP->isBad())continue;int nIDr;// 該地圖點在閉環檢測中被當前KF調整過，那么使用調整它的KF idif(pMP->mnCorrectedByKF==pCurKF->mnId){nIDr = pMP->mnCorrectedReference;}else{// 通常情況下地圖點的參考關鍵幀就是創建該地圖點的那個關鍵幀KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();nIDr = pRefKF->mnId;}// 得到地圖點參考關鍵幀優化前的位姿g2o::Sim3 Srw = vScw[nIDr];// 得到地圖點參考關鍵幀優化后的位姿g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];cv::Mat P3Dw = pMP->GetWorldPos();Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);// 這里優化后的位置也是直接寫入到地圖點之中的pMP->SetWorldPos(cvCorrectedP3Dw);// 記得更新一下pMP->UpdateNormalAndDepth();} // 使用相對位姿變換的方法來更新地圖點的位姿
}

5.8 KeyFrame`的用途

KeyFrame的創建:

Tracking線程中通過函數Tracking::NeedNewKeyFrame()判斷是否需要關鍵幀,若需要關鍵幀,則調用函數Tracking::CreateNewKeyFrame()創建關鍵幀.

KeyFrame的銷毀:

LocalMapping線程剔除冗余關鍵幀函數LocalMapping::KeyFrameCulling()中若檢查到某關鍵幀為冗余關鍵幀,則調用函數KeyFrame::SetBadFlag()刪除關鍵幀.

6. ORB-SLAM2代碼詳解06_單目初始化器Initializer（對應文件Initializer.cc、Initializer.h)

6.1 各成員變量/函數

Initializer類僅用于單目相機初始化,雙目/RGBD相機初始化不用這個類.

成員變量名中: 1代表參考幀(reference frame)中特征點編號,2代表當前幀(current frame)中特征點編號.

/*** @brief 計算基礎矩陣和單應性矩陣，選取最佳的來恢復出最開始兩幀之間的相對姿態，并進行三角化得到初始地圖點* Step 1 重新記錄特征點對的匹配關系* Step 2 在所有匹配特征點對中隨機選擇8對匹配特征點為一組，用于估計H矩陣和F矩陣* Step 3 計算fundamental 矩陣 和homography 矩陣，為了加速分別開了線程計算 * Step 4 計算得分比例來判斷選取哪個模型來求位姿R,t* * @param[in] CurrentFrame          當前幀，也就是SLAM意義上的第二幀* @param[in] vMatches12            當前幀（2）和參考幀（1）圖像中特征點的匹配關系*                                  vMatches12[i]解釋：i表示幀1中關鍵點的索引值，vMatches12[i]的值為幀2的關鍵點索引值*                                  沒有匹配關系的話，vMatches12[i]值為 -1* @param[in & out] R21                   相機從參考幀到當前幀的旋轉* @param[in & out] t21                   相機從參考幀到當前幀的平移* @param[in & out] vP3D                  三角化測量之后的三維地圖點* @param[in & out] vbTriangulated        標記三角化點是否有效，有效為true* @return true                     該幀可以成功初始化，返回true* @return false                    該幀不滿足初始化條件，返回false*/
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{// Fill structures with current keypoints and matches with reference frame// Reference Frame: 1, Current Frame: 2//獲取當前幀的去畸變之后的特征點mvKeys2 = CurrentFrame.mvKeysUn;// mvMatches12記錄匹配上的特征點對，記錄的是幀2在幀1的匹配索引mvMatches12.clear();// 預分配空間，大小和關鍵點數目一致mvKeys2.size()mvMatches12.reserve(mvKeys2.size());// 記錄參考幀1中的每個特征點是否有匹配的特征點// 這個成員變量后面沒有用到，后面只關心匹配上的特征點 	mvbMatched1.resize(mvKeys1.size());// Step 1 重新記錄特征點對的匹配關系存儲在mvMatches12，是否有匹配存儲在mvbMatched1// 將vMatches12（有冗余） 轉化為 mvMatches12（只記錄了匹配關系）for(size_t i=0, iend=vMatches12.size();i<iend; i++){//vMatches12[i]解釋：i表示幀1中關鍵點的索引值，vMatches12[i]的值為幀2的關鍵點索引值//沒有匹配關系的話，vMatches12[i]值為 -1if(vMatches12[i]>=0){//mvMatches12 中只記錄有匹配關系的特征點對的索引值//i表示幀1中關鍵點的索引值，vMatches12[i]的值為幀2的關鍵點索引值mvMatches12.push_back(make_pair(i,vMatches12[i]));//標記參考幀1中的這個特征點有匹配關系mvbMatched1[i]=true;}else//標記參考幀1中的這個特征點沒有匹配關系mvbMatched1[i]=false;}// 有匹配的特征點的對數const int N = mvMatches12.size();// Indices for minimum set selection// 新建一個容器vAllIndices存儲特征點索引，并預分配空間vector<size_t> vAllIndices;vAllIndices.reserve(N);//在RANSAC的某次迭代中，還可以被抽取來作為數據樣本的特征點對的索引，所以這里起的名字叫做可用的索引vector<size_t> vAvailableIndices;//初始化所有特征點對的索引，索引值0到N-1for(int i=0; i<N; i++){vAllIndices.push_back(i);}// Generate sets of 8 points for each RANSAC iteration// Step 2 在所有匹配特征點對中隨機選擇8對匹配特征點為一組，用于估計H矩陣和F矩陣// 共選擇 mMaxIterations (默認200) 組//mvSets用于保存每次迭代時所使用的向量mvSets = vector< vector<size_t> >(mMaxIterations,		//最大的RANSAC迭代次數vector<size_t>(8,0));	//這個則是第二維元素的初始值，也就是第一維。這里其實也是一個第一維的構造函數，第一維vector有8項，每項的初始值為0.//用于進行隨機數據樣本采樣，設置隨機數種子DUtils::Random::SeedRandOnce(0);//開始每一次的迭代 for(int it=0; it<mMaxIterations; it++){//迭代開始的時候，所有的點都是可用的vAvailableIndices = vAllIndices;// Select a minimum set//選擇最小的數據樣本集，使用八點法求，所以這里就循環了八次for(size_t j=0; j<8; j++){// 隨機產生一對點的id,范圍從0到N-1int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);// idx表示哪一個索引對應的特征點對被選中int idx = vAvailableIndices[randi];//將本次迭代這個選中的第j個特征點對的索引添加到mvSets中mvSets[it][j] = idx;// 由于這對點在本次迭代中已經被使用了,所以我們為了避免再次抽到這個點,就在"點的可選列表"中,// 將這個點原來所在的位置用vector最后一個元素的信息覆蓋,并且刪除尾部的元素// 這樣就相當于將這個點的信息從"點的可用列表"中直接刪除了vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出8個特征點對}//迭代mMaxIterations次，選取各自迭代時需要用到的最小數據集// Launch threads to compute in parallel a fundamental matrix and a homography// Step 3 計算fundamental 矩陣 和homography 矩陣，為了加速分別開了線程計算 //這兩個變量用于標記在H和F的計算中哪些特征點對被認為是Inliervector<bool> vbMatchesInliersH, vbMatchesInliersF;//計算出來的單應矩陣和基礎矩陣的RANSAC評分，這里其實是采用重投影誤差來計算的float SH, SF; //score for H and F//這兩個是經過RANSAC算法后計算出來的單應矩陣和基礎矩陣cv::Mat H, F; // 構造線程來計算H矩陣及其得分// thread方法比較特殊，在傳遞引用的時候，外層需要用ref來進行引用傳遞，否則就是淺拷貝thread threadH(&Initializer::FindHomography,	//該線程的主函數this,							//由于主函數為類的成員函數，所以第一個參數就應該是當前對象的this指針ref(vbMatchesInliersH), 			//輸出，特征點對的Inlier標記ref(SH), 						//輸出，計算的單應矩陣的RANSAC評分ref(H));							//輸出，計算的單應矩陣結果// 計算fundamental matrix并打分，參數定義和H是一樣的，這里不再贅述thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));// Wait until both threads have finished//等待兩個計算線程結束threadH.join();threadF.join();// Compute ratio of scores// Step 4 計算得分比例來判斷選取哪個模型來求位姿R,t//通過這個規則來判斷誰的評分占比更多一些，注意不是簡單的比較絕對評分大小，而是看評分的占比float RH = SH/(SH+SF);			//RH=Ratio of Homography// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)// 注意這里更傾向于用H矩陣恢復位姿。如果單應矩陣的評分占比達到了0.4以上,則從單應矩陣恢復運動,否則從基礎矩陣恢復運動if(RH>0.40)//更偏向于平面，此時從單應矩陣恢復，函數ReconstructH返回bool型結果return ReconstructH(vbMatchesInliersH,	//輸入，匹配成功的特征點對Inliers標記H,					//輸入，前面RANSAC計算后的單應矩陣mK,					//輸入，相機的內參數矩陣R21,t21,			//輸出，計算出來的相機從參考幀1到當前幀2所發生的旋轉和位移變換vP3D,				//特征點對經過三角測量之后的空間坐標，也就是地圖點vbTriangulated,		//特征點對是否成功三角化的標記1.0,				//這個對應的形參為minParallax，即認為某對特征點的三角化測量中，認為其測量有效時//需要滿足的最小視差角（如果視差角過小則會引起非常大的觀測誤差）,單位是角度50);				//為了進行運動恢復，所需要的最少的三角化測量成功的點個數else //if(pF_HF>0.6)// 更偏向于非平面，從基礎矩陣恢復return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);//一般地程序不應該執行到這里，如果執行到這里說明程序跑飛了return false;
}

6.2 計算基礎矩陣F和單應矩陣H

少數外點會極大影響計算結果的準確度，隨著采樣數量的增加，外加數量也會同時增加，這是一種或系統誤差，無法通過增加采樣點來解決。

?RANSAC(Random sample consensus,隨機采樣一致性)算法的思路是少量多次重復實驗,每次實驗僅使用盡可能少的點來計算,并統計本次計算中的內點數.只要嘗試次數足夠多的話,總會找到一個包含所有內點的解.

?RANSAC算法的核心是減少每次迭代所需的采樣點數.從原理上來說,計算F矩陣最少只需要7對匹配點,計算H矩陣最少只需要4對匹配點;ORB-SLAM2中為了編程方便,每次迭代使用8對匹配點計算F和H.

?6.2.2 計算基礎矩陣F:?FindFundamental()

/*** @brief 計算基礎矩陣，假設場景為非平面情況下通過前兩幀求取Fundamental矩陣，得到該模型的評分* Step 1 將當前幀和參考幀中的特征點坐標進行歸一化* Step 2 選擇8個歸一化之后的點對進行迭代* Step 3 八點法計算基礎矩陣矩陣* Step 4 利用重投影誤差為當次RANSAC的結果評分* Step 5 更新具有最優評分的基礎矩陣計算結果,并且保存所對應的特征點對的內點標記* * @param[in & out] vbMatchesInliers          標記是否是外點* @param[in & out] score                     計算基礎矩陣得分* @param[in & out] F21                       從特征點1到2的基礎矩陣*/
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{// 計算基礎矩陣,其過程和上面的計算單應矩陣的過程十分相似.// Number of putative matches// 匹配的特征點對總數// const int N = vbMatchesInliers.size();  // !源代碼出錯！請使用下面代替const int N = mvMatches12.size();// Normalize coordinates// Step 1 將當前幀和參考幀中的特征點坐標進行歸一化，主要是平移和尺度變換// 具體來說,就是將mvKeys1和mvKey2歸一化到均值為0，一階絕對矩為1，歸一化矩陣分別為T1、T2// 這里所謂的一階絕對矩其實就是隨機變量到取值的中心的絕對值的平均值// 歸一化矩陣就是把上述歸一化的操作用矩陣來表示。這樣特征點坐標乘歸一化矩陣可以得到歸一化后的坐標vector<cv::Point2f> vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);// ! 注意這里取的是歸一化矩陣T2的轉置,因為基礎矩陣的定義和單應矩陣不同，兩者去歸一化的計算也不相同cv::Mat T2t = T2.t();// Best Results variables//最優結果score = 0.0;vbMatchesInliers = vector<bool>(N,false);// Iteration variables// 某次迭代中，參考幀的特征點坐標vector<cv::Point2f> vPn1i(8);// 某次迭代中，當前幀的特征點坐標vector<cv::Point2f> vPn2i(8);// 某次迭代中，計算的基礎矩陣cv::Mat F21i;// 每次RANSAC記錄的Inliers與得分vector<bool> vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score// 下面進行每次的RANSAC迭代for(int it=0; it<mMaxIterations; it++){// Select a minimum set// Step 2 選擇8個歸一化之后的點對進行迭代for(int j=0; j<8; j++){int idx = mvSets[it][j];// vPn1i和vPn2i為匹配的特征點對的歸一化后的坐標// 首先根據這個特征點對的索引信息分別找到兩個特征點在各自圖像特征點向量中的索引，然后讀取其歸一化之后的特征點坐標vPn1i[j] = vPn1[mvMatches12[idx].first];        //first存儲在參考幀1中的特征點索引vPn2i[j] = vPn2[mvMatches12[idx].second];       //second存儲在參考幀1中的特征點索引}// Step 3 八點法計算基礎矩陣cv::Mat Fn = ComputeF21(vPn1i,vPn2i);// 基礎矩陣約束：p2^t*F21*p1 = 0，其中p1,p2 為齊次化特征點坐標    // 特征點歸一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  // 根據基礎矩陣約束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0   // 進一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0F21i = T2t*Fn*T1;// Step 4 利用重投影誤差為當次RANSAC的結果評分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);// Step 5 更新具有最優評分的基礎矩陣計算結果,并且保存所對應的特征點對的內點標記if(currentScore>score){//如果當前的結果得分更高，那么就更新最優計算結果F21 = F21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}}
}

6.2.3 八點法計算F矩陣:?ComputeF21()

展開成

??一對點提供兩個約束等式，單應矩陣H總共有9個元素，8個自由度（尺度等價性），所以需要4對點提供
8個約束方程就可以求解。?

?上圖中 矩陣是一個 的矩陣, 是一個 的向量;上述方程是一個超定方程,使用SVD分解求最小二乘解.

/*** @brief 根據特征點匹配求fundamental matrix（normalized 8點法）* 注意F矩陣有秩為2的約束，所以需要兩次SVD分解* * @param[in] vP1           參考幀中歸一化后的特征點* @param[in] vP2           當前幀中歸一化后的特征點* @return cv::Mat          最后計算得到的基礎矩陣F*/
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //歸一化后的點, in reference frameconst vector<cv::Point2f> &vP2) //歸一化后的點, in current frame
{// 原理詳見附件推導// x'Fx = 0 整理可得：Af = 0// A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |// 通過SVD求解Af = 0，A'A最小特征值對應的特征向量即為解//獲取參與計算的特征點對數const int N = vP1.size();//初始化A矩陣cv::Mat A(N,9,CV_32F); // N*9維// 構造矩陣A，將每個特征點添加到矩陣A中的元素for(int i=0; i<N; i++){const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;A.at<float>(i,0) = u2*u1;A.at<float>(i,1) = u2*v1;A.at<float>(i,2) = u2;A.at<float>(i,3) = v2*u1;A.at<float>(i,4) = v2*v1;A.at<float>(i,5) = v2;A.at<float>(i,6) = u1;A.at<float>(i,7) = v1;A.at<float>(i,8) = 1;}//存儲奇異值分解結果的變量cv::Mat u,w,vt;// 定義輸出變量，u是左邊的正交矩陣U， w為奇異矩陣，vt中的t表示是右正交矩陣V的轉置cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 轉換成基礎矩陣的形式cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列//基礎矩陣的秩為2,而我們不敢保證計算得到的這個結果的秩為2,所以需要通過第二次奇異值分解,來強制使其秩為2// 對初步得來的基礎矩陣進行第2次奇異值分解cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 秩2約束，強制將第3個奇異值設置為0w.at<float>(2)=0; // 重新組合好滿足秩約束的基礎矩陣，作為最終計算結果返回 return  u*cv::Mat::diag(w)*vt;
}

6.2.4 計算單應矩陣H:?FindHomography()

以下兩種情況更適合使用單應矩陣進行初始化:

1. 相機看到的場景是一個平面.
2. 連續兩幀間沒發生平移,只發生旋轉.

?

?使用八點法求解單應矩陣H的原理類似:

/*** @brief 計算單應矩陣，假設場景為平面情況下通過前兩幀求取Homography矩陣，并得到該模型的評分* 原理參考Multiple view geometry in computer vision  P109 算法4.4* Step 1 將當前幀和參考幀中的特征點坐標進行歸一化* Step 2 選擇8個歸一化之后的點對進行迭代* Step 3 八點法計算單應矩陣矩陣* Step 4 利用重投影誤差為當次RANSAC的結果評分* Step 5 更新具有最優評分的單應矩陣計算結果,并且保存所對應的特征點對的內點標記* * @param[in & out] vbMatchesInliers          標記是否是外點* @param[in & out] score                     計算單應矩陣的得分* @param[in & out] H21                       單應矩陣結果*/
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{// Number of putative matches//匹配的特征點對總數const int N = mvMatches12.size();// Normalize coordinates// Step 1 將當前幀和參考幀中的特征點坐標進行歸一化，主要是平移和尺度變換// 具體來說,就是將mvKeys1和mvKey2歸一化到均值為0，一階絕對矩為1，歸一化矩陣分別為T1、T2// 這里所謂的一階絕對矩其實就是隨機變量到取值的中心的絕對值的平均值// 歸一化矩陣就是把上述歸一化的操作用矩陣來表示。這樣特征點坐標乘歸一化矩陣可以得到歸一化后的坐標//歸一化后的參考幀1和當前幀2中的特征點坐標vector<cv::Point2f> vPn1, vPn2;// 記錄各自的歸一化矩陣cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);//這里求的逆在后面的代碼中要用到，輔助進行原始尺度的恢復cv::Mat T2inv = T2.inv();// Best Results variables// 記錄最佳評分score = 0.0;// 取得歷史最佳評分時,特征點對的inliers標記vbMatchesInliers = vector<bool>(N,false);// Iteration variables//某次迭代中，參考幀的特征點坐標vector<cv::Point2f> vPn1i(8);//某次迭代中，當前幀的特征點坐標vector<cv::Point2f> vPn2i(8);//以及計算出來的單應矩陣、及其逆矩陣cv::Mat H21i, H12i;// 每次RANSAC記錄Inliers與得分vector<bool> vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score//下面進行每次的RANSAC迭代for(int it=0; it<mMaxIterations; it++){// Select a minimum set// Step 2 選擇8個歸一化之后的點對進行迭代for(size_t j=0; j<8; j++){//從mvSets中獲取當前次迭代的某個特征點對的索引信息int idx = mvSets[it][j];// vPn1i和vPn2i為匹配的特征點對的歸一化后的坐標// 首先根據這個特征點對的索引信息分別找到兩個特征點在各自圖像特征點向量中的索引，然后讀取其歸一化之后的特征點坐標vPn1i[j] = vPn1[mvMatches12[idx].first];    //first存儲在參考幀1中的特征點索引vPn2i[j] = vPn2[mvMatches12[idx].second];   //second存儲在參考幀1中的特征點索引}//讀取8對特征點的歸一化之后的坐標// Step 3 八點法計算單應矩陣// 利用生成的8個歸一化特征點對, 調用函數 Initializer::ComputeH21() 使用八點法計算單應矩陣  // 關于為什么計算之前要對特征點進行歸一化，后面又恢復這個矩陣的尺度？// 可以在《計算機視覺中的多視圖幾何》這本書中P193頁中找到答案// 書中這里說,8點算法成功的關鍵是在構造解的方稱之前應對輸入的數據認真進行適當的歸一化cv::Mat Hn = ComputeH21(vPn1i,vPn2i);// 單應矩陣原理：X2=H21*X1，其中X1,X2 為歸一化后的特征點    // 特征點歸一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  得到:T2 * mvKeys2 =  Hn * T1 * mvKeys1   // 進一步得到:mvKeys2  = T2.inv * Hn * T1 * mvKeys1H21i = T2inv*Hn*T1;//然后計算逆H12i = H21i.inv();// Step 4 利用重投影誤差為當次RANSAC的結果評分currentScore = CheckHomography(H21i, H12i, 			//輸入，單應矩陣的計算結果vbCurrentInliers, 	//輸出，特征點對的Inliers標記mSigma);				//TODO  測量誤差，在Initializer類對象構造的時候，由外部給定的// Step 5 更新具有最優評分的單應矩陣計算結果,并且保存所對應的特征點對的內點標記if(currentScore>score){//如果當前的結果得分更高，那么就更新最優計算結果H21 = H21i.clone();//保存匹配好的特征點對的Inliers標記vbMatchesInliers = vbCurrentInliers;//更新歷史最優評分score = currentScore;}}
}

/*** @brief 用DLT方法求解單應矩陣H* 這里最少用4對點就能夠求出來，不過這里為了統一還是使用了8對點求最小二乘解* * @param[in] vP1               參考幀中歸一化后的特征點* @param[in] vP2               當前幀中歸一化后的特征點* @return cv::Mat              計算的單應矩陣H*/
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, //歸一化后的點, in reference frameconst vector<cv::Point2f> &vP2) //歸一化后的點, in current frame
{// 基本原理：見附件推導過程：// |x'|     | h1 h2 h3 ||x|// |y'| = a | h4 h5 h6 ||y|  簡寫: x' = a H x, a為一個尺度因子// |1 |     | h7 h8 h9 ||1|// 使用DLT(direct linear tranform)求解該模型// x' = a H x // ---> (x') 叉乘 (H x)  = 0  (因為方向相同) (取前兩行就可以推導出下面的了)// ---> Ah = 0 // A = | 0  0  0 -x -y -1 xy' yy' y'|  h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 |//     |-x -y -1  0  0  0 xx' yx' x'|// 通過SVD求解Ah = 0，A^T*A最小特征值對應的特征向量即為解// 其實也就是右奇異值矩陣的最后一列//獲取參與計算的特征點的數目const int N = vP1.size();// 構造用于計算的矩陣 A cv::Mat A(2*N,				//行，注意每一個點的數據對應兩行9,				//列CV_32F);      	//float數據類型// 構造矩陣A，將每個特征點添加到矩陣A中的元素for(int i=0; i<N; i++){//獲取特征點對的像素坐標const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;//生成這個點的第一行A.at<float>(2*i,0) = 0.0;A.at<float>(2*i,1) = 0.0;A.at<float>(2*i,2) = 0.0;A.at<float>(2*i,3) = -u1;A.at<float>(2*i,4) = -v1;A.at<float>(2*i,5) = -1;A.at<float>(2*i,6) = v2*u1;A.at<float>(2*i,7) = v2*v1;A.at<float>(2*i,8) = v2;//生成這個點的第二行A.at<float>(2*i+1,0) = u1;A.at<float>(2*i+1,1) = v1;A.at<float>(2*i+1,2) = 1;A.at<float>(2*i+1,3) = 0.0;A.at<float>(2*i+1,4) = 0.0;A.at<float>(2*i+1,5) = 0.0;A.at<float>(2*i+1,6) = -u2*u1;A.at<float>(2*i+1,7) = -u2*v1;A.at<float>(2*i+1,8) = -u2;}// 定義輸出變量，u是左邊的正交矩陣U， w為奇異矩陣，vt中的t表示是右正交矩陣V的轉置cv::Mat u,w,vt;//使用opencv提供的進行奇異值分解的函數cv::SVDecomp(A,							//輸入，待進行奇異值分解的矩陣w,							//輸出，奇異值矩陣u,							//輸出，矩陣Uvt,						//輸出，矩陣V^Tcv::SVD::MODIFY_A | 		//輸入，MODIFY_A是指允許計算函數可以修改待分解的矩陣，官方文檔上說這樣可以加快計算速度、節省內存cv::SVD::FULL_UV);		//FULL_UV=把U和VT補充成單位正交方陣// 返回最小奇異值所對應的右奇異向量// 注意前面說的是右奇異值矩陣的最后一列，但是在這里因為是vt，轉置后了，所以是行；由于A有9列數據，故最后一列的下標為8return vt.row(8).reshape(0, 			//轉換后的通道數，這里設置為0表示是與前面相同3); 			//轉換后的行數,對應V的最后一列
}

6.2.5 卡方檢驗計算置信度得分: CheckFundamental()、CheckHomography()

卡方檢驗通過構造檢驗統計量來比較期望結果和實際結果之間的差別,從而得出觀察頻數極值的發生概率.

根據重投影誤差構造統計量,其值越大,觀察結果和期望結果之間的差別越顯著,某次計算越可能用到了外點.

?統計量置信度閾值與被檢驗變量自由度有關: 單目特征點重投影誤差的自由度為2(u,v),雙目特征點重投影誤差自由度為3(u,v,ur).

取95%置信度下的卡方檢驗統計量閾值

若統計量大于該閾值,則認為計算矩陣使用到了外點,將其分數設為0.
若統計量小于該閾值,則將統計量裕量設為該解的置信度分數.

/*** @brief 對給定的homography matrix打分,需要使用到卡方檢驗的知識* * @param[in] H21                       從參考幀到當前幀的單應矩陣* @param[in] H12                       從當前幀到參考幀的單應矩陣* @param[in] vbMatchesInliers          匹配好的特征點對的Inliers標記* @param[in] sigma                     方差，默認為1* @return float                        返回得分*/
float Initializer::CheckHomography(const cv::Mat &H21,                 //從參考幀到當前幀的單應矩陣const cv::Mat &H12,                 //從當前幀到參考幀的單應矩陣vector<bool> &vbMatchesInliers,     //匹配好的特征點對的Inliers標記float sigma)                        //估計誤差
{// 說明：在已值n維觀測數據誤差服從N(0，sigma）的高斯分布時// 其誤差加權最小二乘結果為  sum_error = SUM(e(i)^T * Q^(-1) * e(i))// 其中：e(i) = [e_x,e_y,...]^T, Q維觀測數據協方差矩陣，即sigma * sigma組成的協方差矩陣// 誤差加權最小二次結果越小，說明觀測數據精度越高// 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分數就越高// 算法目標： 檢查單應變換矩陣// 檢查方式：通過H矩陣，進行參考幀和當前幀之間的雙向投影，并計算起加權最小二乘投影誤差// 算法流程// input: 單應性矩陣 H21, H12, 匹配點集 mvKeys1//    do://        for p1(i), p2(i) in mvKeys://           error_i1 = ||p2(i) - H21 * p1(i)||2//           error_i2 = ||p1(i) - H12 * p2(i)||2//           //           w1 = 1 / sigma / sigma//           w2 = 1 / sigma / sigma// //           if error1 < th//              score +=   th - error_i1 * w1//           if error2 < th//              score +=   th - error_i2 * w2// //           if error_1i > th or error_2i > th//              p1(i), p2(i) are inner points//              vbMatchesInliers(i) = true//           else //              p1(i), p2(i) are outliers//              vbMatchesInliers(i) = false//           end//        end//   output: score, inliers// 特點匹配個數const int N = mvMatches12.size();// Step 1 獲取從參考幀到當前幀的單應矩陣的各個元素const float h11 = H21.at<float>(0,0);const float h12 = H21.at<float>(0,1);const float h13 = H21.at<float>(0,2);const float h21 = H21.at<float>(1,0);const float h22 = H21.at<float>(1,1);const float h23 = H21.at<float>(1,2);const float h31 = H21.at<float>(2,0);const float h32 = H21.at<float>(2,1);const float h33 = H21.at<float>(2,2);// 獲取從當前幀到參考幀的單應矩陣的各個元素const float h11inv = H12.at<float>(0,0);const float h12inv = H12.at<float>(0,1);const float h13inv = H12.at<float>(0,2);const float h21inv = H12.at<float>(1,0);const float h22inv = H12.at<float>(1,1);const float h23inv = H12.at<float>(1,2);const float h31inv = H12.at<float>(2,0);const float h32inv = H12.at<float>(2,1);const float h33inv = H12.at<float>(2,2);// 給特征點對的Inliers標記預分配空間vbMatchesInliers.resize(N);// 初始化score值float score = 0;// 基于卡方檢驗計算出的閾值（假設測量有一個像素的偏差）// 自由度為2的卡方分布，顯著性水平為0.05，對應的臨界閾值const float th = 5.991;//信息矩陣，方差平方的倒數const float invSigmaSquare = 1.0/(sigma * sigma);// Step 2 通過H矩陣，進行參考幀和當前幀之間的雙向投影，并計算起加權重投影誤差// H21 表示從img1 到 img2的變換矩陣// H12 表示從img2 到 img1的變換矩陣 for(int i = 0; i < N; i++){// 一開始都默認為Inlierbool bIn = true;// Step 2.1 提取參考幀和當前幀之間的特征匹配點對const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Step 2.2 計算 img2 到 img1 的重投影誤差// x1 = H12*x2// 將圖像2中的特征點通過單應變換投影到圖像1中// |u1|   |h11inv h12inv h13inv||u2|   |u2in1|// |v1| = |h21inv h22inv h23inv||v2| = |v2in1| * w2in1inv// |1 |   |h31inv h32inv h33inv||1 |   |  1  |// 計算投影歸一化坐標const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv);const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;// 計算重投影誤差 = ||p1(i) - H12 * p2(i)||2const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);const float chiSquare1 = squareDist1 * invSigmaSquare;// Step 2.3 用閾值標記離群點，內點的話累加得分if(chiSquare1>th)bIn = false;    else// 誤差越大，得分越低score += th - chiSquare1;// 計算從img1 到 img2 的投影變換誤差// x1in2 = H21*x1// 將圖像2中的特征點通過單應變換投影到圖像1中// |u2|   |h11 h12 h13||u1|   |u1in2|// |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv// |1 |   |h31 h32 h33||1 |   |  1  |// 計算投影歸一化坐標const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;// 計算重投影誤差 const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);const float chiSquare2 = squareDist2*invSigmaSquare;// 用閾值標記離群點，內點的話累加得分if(chiSquare2>th)bIn = false;elsescore += th - chiSquare2;   // Step 2.4 如果從img2 到 img1 和 從img1 到img2的重投影誤差均滿足要求，則說明是Inlier pointif(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}return score;
}

/*** @brief 對給定的Fundamental matrix打分* * @param[in] F21                       當前幀和參考幀之間的基礎矩陣* @param[in] vbMatchesInliers          匹配的特征點對屬于inliers的標記* @param[in] sigma                     方差，默認為1* @return float                        返回得分*/
float Initializer::CheckFundamental(const cv::Mat &F21,             //當前幀和參考幀之間的基礎矩陣vector<bool> &vbMatchesInliers, //匹配的特征點對屬于inliers的標記float sigma)                    //方差
{// 說明：在已值n維觀測數據誤差服從N(0，sigma）的高斯分布時// 其誤差加權最小二乘結果為  sum_error = SUM(e(i)^T * Q^(-1) * e(i))// 其中：e(i) = [e_x,e_y,...]^T, Q維觀測數據協方差矩陣，即sigma * sigma組成的協方差矩陣// 誤差加權最小二次結果越小，說明觀測數據精度越高// 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分數就越高// 算法目標：檢查基礎矩陣// 檢查方式：利用對極幾何原理 p2^T * F * p1 = 0// 假設：三維空間中的點 P 在 img1 和 img2 兩圖像上的投影分別為 p1 和 p2（兩個為同名點）//   則：p2 一定存在于極線 l2 上，即 p2*l2 = 0. 而l2 = F*p1 = (a, b, c)^T//      所以，這里的誤差項 e 為 p2 到 極線 l2 的距離，如果在直線上，則 e = 0//      根據點到直線的距離公式：d = (ax + by + c) / sqrt(a * a + b * b)//      所以，e =  (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)// 算法流程// input: 基礎矩陣 F 左右視圖匹配點集 mvKeys1//    do://        for p1(i), p2(i) in mvKeys://           l2 = F * p1(i)//           l1 = p2(i) * F//           error_i1 = dist_point_to_line(x2,l2)//           error_i2 = dist_point_to_line(x1,l1)//           //           w1 = 1 / sigma / sigma//           w2 = 1 / sigma / sigma// //           if error1 < th//              score +=   thScore - error_i1 * w1//           if error2 < th//              score +=   thScore - error_i2 * w2// //           if error_1i > th or error_2i > th//              p1(i), p2(i) are inner points//              vbMatchesInliers(i) = true//           else //              p1(i), p2(i) are outliers//              vbMatchesInliers(i) = false//           end//        end//   output: score, inliers// 獲取匹配的特征點對的總對數const int N = mvMatches12.size();// Step 1 提取基礎矩陣中的元素數據const float f11 = F21.at<float>(0,0);const float f12 = F21.at<float>(0,1);const float f13 = F21.at<float>(0,2);const float f21 = F21.at<float>(1,0);const float f22 = F21.at<float>(1,1);const float f23 = F21.at<float>(1,2);const float f31 = F21.at<float>(2,0);const float f32 = F21.at<float>(2,1);const float f33 = F21.at<float>(2,2);// 預分配空間vbMatchesInliers.resize(N);// 設置評分初始值（因為后面需要進行這個數值的累計）float score = 0;// 基于卡方檢驗計算出的閾值// 自由度為1的卡方分布，顯著性水平為0.05，對應的臨界閾值// ?是因為點到直線距離是一個自由度嗎？const float th = 3.841;// 自由度為2的卡方分布，顯著性水平為0.05，對應的臨界閾值const float thScore = 5.991;// 信息矩陣，或 協方差矩陣的逆矩陣const float invSigmaSquare = 1.0/(sigma*sigma);// Step 2 計算img1 和 img2 在估計 F 時的score值for(int i=0; i<N; i++){//默認為這對特征點是Inliersbool bIn = true;// Step 2.1 提取參考幀和當前幀之間的特征匹配點對const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];// 提取出特征點的坐標const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Reprojection error in second image// Step 2.2 計算 img1 上的點在 img2 上投影得到的極線 l2 = F21 * p1 = (a2,b2,c2)const float a2 = f11*u1+f12*v1+f13;const float b2 = f21*u1+f22*v1+f23;const float c2 = f31*u1+f32*v1+f33;// Step 2.3 計算誤差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num2 = a2*u2+b2*v2+c2;const float squareDist1 = num2*num2/(a2*a2+b2*b2);// 帶權重誤差const float chiSquare1 = squareDist1*invSigmaSquare;// Step 2.4 誤差大于閾值就說明這個點是Outlier // ? 為什么判斷閾值用的 th（1自由度），計算得分用的thScore（2自由度）// ? 可能是為了和CheckHomography 得分統一？if(chiSquare1>th)bIn = false;else// 誤差越大，得分越低score += thScore - chiSquare1;// 計算img2上的點在 img1 上投影得到的極線 l1= p2 * F21 = (a1,b1,c1)const float a1 = f11*u2+f21*v2+f31;const float b1 = f12*u2+f22*v2+f32;const float c1 = f13*u2+f23*v2+f33;// 計算誤差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num1 = a1*u1+b1*v1+c1;const float squareDist2 = num1*num1/(a1*a1+b1*b1);// 帶權重誤差const float chiSquare2 = squareDist2*invSigmaSquare;// 誤差大于閾值就說明這個點是Outlier if(chiSquare2>th)bIn = false;elsescore += thScore - chiSquare2;// Step 2.5 保存結果if(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}//  返回評分return score;
}

6.2.6 歸一化:?Normalize()

使用均值和一階中心矩歸一化,歸一化可以增強計算穩定性.

/*** @brief 歸一化特征點到同一尺度，作為后續normalize DLT的輸入*  [x' y' 1]' = T * [x y 1]' *  歸一化后x', y'的均值為0，sum(abs(x_i'-0))=1，sum(abs((y_i'-0))=1**  為什么要歸一化？*  在相似變換之后(點在不同的坐標系下),他們的單應性矩陣是不相同的*  如果圖像存在噪聲,使得點的坐標發生了變化,那么它的單應性矩陣也會發生變化*  我們采取的方法是將點的坐標放到同一坐標系下,并將縮放尺度也進行統一 *  對同一幅圖像的坐標進行相同的變換,不同圖像進行不同變換*  縮放尺度是為了讓噪聲對于圖像的影響在一個數量級上* *  Step 1 計算特征點X,Y坐標的均值 *  Step 2 計算特征點X,Y坐標離均值的平均偏離程度*  Step 3 將x坐標和y坐標分別進行尺度歸一化，使得x坐標和y坐標的一階絕對矩分別為1 *  Step 4 計算歸一化矩陣：其實就是前面做的操作用矩陣變換來表示而已* * @param[in] vKeys                               待歸一化的特征點* @param[in & out] vNormalizedPoints             特征點歸一化后的坐標* @param[in & out] T                             歸一化特征點的變換矩陣*/
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)                           //將特征點歸一化的矩陣
{// 歸一化的是這些點在x方向和在y方向上的一階絕對矩（隨機變量的期望）。// Step 1 計算特征點X,Y坐標的均值 meanX, meanYfloat meanX = 0;float meanY = 0;//獲取特征點的數量const int N = vKeys.size();//設置用來存儲歸一后特征點的向量大小，和歸一化前保持一致vNormalizedPoints.resize(N);//開始遍歷所有的特征點for(int i=0; i<N; i++){//分別累加特征點的X、Y坐標meanX += vKeys[i].pt.x;meanY += vKeys[i].pt.y;}//計算X、Y坐標的均值meanX = meanX/N;meanY = meanY/N;// Step 2 計算特征點X,Y坐標離均值的平均偏離程度 meanDevX, meanDevY，注意不是標準差float meanDevX = 0;float meanDevY = 0;// 將原始特征點減去均值坐標，使x坐標和y坐標均值分別為0for(int i=0; i<N; i++){vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;//累計這些特征點偏離橫縱坐標均值的程度meanDevX += fabs(vNormalizedPoints[i].x);meanDevY += fabs(vNormalizedPoints[i].y);}// 求出平均到每個點上，其坐標偏離橫縱坐標均值的程度；將其倒數作為一個尺度縮放因子meanDevX = meanDevX/N;meanDevY = meanDevY/N;float sX = 1.0/meanDevX;float sY = 1.0/meanDevY;// Step 3 將x坐標和y坐標分別進行尺度歸一化，使得x坐標和y坐標的一階絕對矩分別為1 // 這里所謂的一階絕對矩其實就是隨機變量到取值的中心的絕對值的平均值（期望）for(int i=0; i<N; i++){//對，就是簡單地對特征點的坐標進行進一步的縮放vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;}// Step 4 計算歸一化矩陣：其實就是前面做的操作用矩陣變換來表示而已// |sX  0  -meanx*sX|// |0   sY -meany*sY|// |0   0      1    |T = cv::Mat::eye(3,3,CV_32F);T.at<float>(0,0) = sX;T.at<float>(1,1) = sY;T.at<float>(0,2) = -meanX*sX;T.at<float>(1,2) = -meanY*sY;
}

6.3 使用基礎矩陣F和單應矩陣H恢復運動

6.3.1 使用基礎矩陣F恢復運動: ReconstructF()

6.3.2 使用基礎矩陣H恢復運動: ReconstructH()

使用基礎矩陣F分解R、t,數學上會得到四個可能的解,因此分解后調用函數Initializer::CheckRT()檢驗分解結果,取相機前方成功三角化數目最多的一組解.

?

/*** @brief 用H矩陣恢復R, t和三維點* H矩陣分解常見有兩種方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition* 代碼使用了Faugeras SVD-based decomposition算法，參考文獻* Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988 * * @param[in] vbMatchesInliers          匹配點對的內點標記* @param[in] H21                       從參考幀到當前幀的單應矩陣* @param[in] K                         相機的內參數矩陣* @param[in & out] R21                 計算出來的相機旋轉* @param[in & out] t21                 計算出來的相機平移* @param[in & out] vP3D                世界坐標系下，三角化測量特征點對之后得到的特征點的空間坐標* @param[in & out] vbTriangulated      特征點是否成功三角化的標記* @param[in] minParallax               對特征點的三角化測量中，認為其測量有效時需要滿足的最小視差角（如果視差角過小則會引起非常大的觀測誤差）,單位是角度* @param[in] minTriangulated           為了進行運動恢復，所需要的最少的三角化測量成功的點個數* @return true                         單應矩陣成功計算出位姿和三維點* @return false                        初始化失敗*/
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{// 目的 ：通過單應矩陣H恢復兩幀圖像之間的旋轉矩陣R和平移向量T// 參考 ：Motion and structure from motion in a piecewise plannar environment.//        International Journal of Pattern Recognition and Artificial Intelligence, 1988// https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment// 流程://      1. 根據H矩陣的奇異值d'= d2 或者 d' = -d2 分別計算 H 矩陣分解的 8 組解//        1.1 討論 d' > 0 時的 4 組解//        1.2 討論 d' < 0 時的 4 組解//      2. 對 8 組解進行驗證，并選擇產生相機前方最多3D點的解為最優解// 統計匹配的特征點對中屬于內點(Inlier)或有效點個數int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)if(vbMatchesInliers[i])N++;// We recover 8 motion hypotheses using the method of Faugeras et al.// Motion and structure from motion in a piecewise planar environment.// International Journal of Pattern Recognition and Artificial Intelligence, 1988// 參考SLAM十四講第二版p170-p171// H = K * (R - t * n / d) * K_inv// 其中: K表示內參數矩陣//       K_inv 表示內參數矩陣的逆//       R 和 t 表示旋轉和平移向量//       n 表示平面法向量// 令 H = K * A * K_inv// 則 A = k_inv * H * kcv::Mat invK = K.inv();cv::Mat A = invK*H21*K;// 對矩陣A進行SVD分解// A 等待被進行奇異值分解的矩陣// w 奇異值矩陣// U 奇異值分解左矩陣// Vt 奇異值分解右矩陣，注意函數返回的是轉置// cv::SVD::FULL_UV 全部分解// A = U * w * Vtcv::Mat U,w,Vt,V;cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);// 根據文獻eq(8)，計算關聯變量V=Vt.t();// 計算變量s = det(U) * det(V)// 因為det(V)==det(Vt), 所以 s = det(U) * det(Vt)float s = cv::determinant(U)*cv::determinant(Vt);// 取得矩陣的各個奇異值float d1 = w.at<float>(0);float d2 = w.at<float>(1);float d3 = w.at<float>(2);// SVD分解正常情況下特征值di應該是正的，且滿足d1>=d2>=d3if(d1/d2<1.00001 || d2/d3<1.00001) {return false;}// 在ORBSLAM中沒有對奇異值 d1 d2 d3按照論文中描述的關系進行分類討論, 而是直接進行了計算// 定義8中情況下的旋轉矩陣、平移向量和空間向量vector<cv::Mat> vR, vt, vn;vR.reserve(8);vt.reserve(8);vn.reserve(8);// Step 1.1 討論 d' > 0 時的 4 組解// 根據論文eq.(12)有// x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))// x2 = 0// x3 = e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3))// 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))//    aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))// 則// x1 = e1 * aux1// x3 = e3 * aux2// 因為 e1,e2,e3 = 1 or -1// 所以有x1和x3有四種組合// x1 =  {aux1,aux1,-aux1,-aux1}// x3 =  {aux3,-aux3,aux3,-aux3}float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));float x1[] = {aux1,aux1,-aux1,-aux1};float x3[] = {aux3,-aux3,aux3,-aux3};// 根據論文eq.(13)有// sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2// cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2 // 令  aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2)// 則  sin(theta) = e1 * e3 * aux_stheta//     cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2)// 因為 e1 e2 e3 = 1 or -1// 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};// 計算旋轉矩陣 R'//根據不同的e1 e3組合所得出來的四種R t的解//      | ctheta      0   -aux_stheta|       | aux1|// Rp = |    0        1       0      |  tp = |  0  |//      | aux_stheta  0    ctheta    |       |-aux3|//      | ctheta      0    aux_stheta|       | aux1|// Rp = |    0        1       0      |  tp = |  0  |//      |-aux_stheta  0    ctheta    |       | aux3|//      | ctheta      0    aux_stheta|       |-aux1|// Rp = |    0        1       0      |  tp = |  0  |//      |-aux_stheta  0    ctheta    |       |-aux3|//      | ctheta      0   -aux_stheta|       |-aux1|// Rp = |    0        1       0      |  tp = |  0  |//      | aux_stheta  0    ctheta    |       | aux3|// 開始遍歷這四種情況中的每一種for(int i=0; i<4; i++){//生成Rp，就是eq.(8) 的 R'cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);Rp.at<float>(0,0)=ctheta;Rp.at<float>(0,2)=-stheta[i];Rp.at<float>(2,0)=stheta[i];        Rp.at<float>(2,2)=ctheta;// eq.(8) 計算Rcv::Mat R = s*U*Rp*Vt;// 保存vR.push_back(R);// eq. (14) 生成tp cv::Mat tp(3,1,CV_32F);tp.at<float>(0)=x1[i];tp.at<float>(1)=0;tp.at<float>(2)=-x3[i];tp*=d1-d3;// 這里雖然對t有歸一化，并沒有決定單目整個SLAM過程的尺度// 因為CreateInitialMapMonocular函數對3D點深度會縮放，然后反過來對 t 有改變// eq.(8)恢復原始的tcv::Mat t = U*tp;vt.push_back(t/cv::norm(t));// 構造法向量npcv::Mat np(3,1,CV_32F);np.at<float>(0)=x1[i];np.at<float>(1)=0;np.at<float>(2)=x3[i];// eq.(8) 恢復原始的法向量cv::Mat n = V*np;//看PPT 16頁的圖，保持平面法向量向上if(n.at<float>(2)<0)n=-n;// 添加到vectorvn.push_back(n);}// Step 1.2 討論 d' < 0 時的 4 組解float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);// cos_theta項float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);// 考慮到e1,e2的取值，這里的sin_theta有兩種可能的解float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};// 對于每種由e1 e3取值的組合而形成的四種解的情況for(int i=0; i<4; i++){// 計算旋轉矩陣 R'cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);Rp.at<float>(0,0)=cphi;Rp.at<float>(0,2)=sphi[i];Rp.at<float>(1,1)=-1;Rp.at<float>(2,0)=sphi[i];Rp.at<float>(2,2)=-cphi;// 恢復出原來的Rcv::Mat R = s*U*Rp*Vt;// 然后添加到vector中vR.push_back(R);// 構造tpcv::Mat tp(3,1,CV_32F);tp.at<float>(0)=x1[i];tp.at<float>(1)=0;tp.at<float>(2)=x3[i];tp*=d1+d3;// 恢復出原來的tcv::Mat t = U*tp;// 歸一化之后加入到vector中,要提供給上面的平移矩陣都是要進行過歸一化的vt.push_back(t/cv::norm(t));// 構造法向量npcv::Mat np(3,1,CV_32F);np.at<float>(0)=x1[i];np.at<float>(1)=0;np.at<float>(2)=x3[i];// 恢復出原來的法向量cv::Mat n = V*np;// 保證法向量指向上方if(n.at<float>(2)<0)n=-n;// 添加到vector中vn.push_back(n);}// 最好的good點int bestGood = 0;// 其次最好的good點int secondBestGood = 0;    // 最好的解的索引，初始值為-1int bestSolutionIdx = -1;// 最大的視差角float bestParallax = -1;// 存儲最好解對應的，對特征點對進行三角化測量的結果vector<cv::Point3f> bestP3D;// 最佳解所對應的，那些可以被三角化測量的點的標記vector<bool> bestTriangulated;// Instead of applying the visibility constraints proposed in the WFaugeras' paper (which could fail for points seen with low parallax)// We reconstruct all hypotheses and check in terms of triangulated points and parallax// Step 2. 對 8 組解進行驗證，并選擇產生相機前方最多3D點的解為最優解for(size_t i=0; i<8; i++){// 第i組解對應的比較大的視差角float parallaxi;// 三角化測量之后的特征點的空間坐標vector<cv::Point3f> vP3Di;// 特征點對是否被三角化的標記vector<bool> vbTriangulatedi;// 調用 Initializer::CheckRT(), 計算good點的數目int nGood = CheckRT(vR[i],vt[i],                    //當前組解的旋轉矩陣和平移向量mvKeys1,mvKeys2,                //特征點mvMatches12,vbMatchesInliers,   //特征匹配關系以及Inlier標記K,                              //相機的內參數矩陣vP3Di,                          //存儲三角化測量之后的特征點空間坐標的4.0*mSigma2,                    //三角化過程中允許的最大重投影誤差vbTriangulatedi,                //特征點是否被成功進行三角測量的標記parallaxi);                     // 這組解在三角化測量的時候的比較大的視差角// 更新歷史最優和次優的解// 保留最優的和次優的解.保存次優解的目的是看看最優解是否突出if(nGood>bestGood){// 如果當前組解的good點數是歷史最優，那么之前的歷史最優就變成了歷史次優secondBestGood = bestGood;// 更新歷史最優點bestGood = nGood;// 最優解的組索引為i（就是當前次遍歷）bestSolutionIdx = i;// 更新變量bestParallax = parallaxi;bestP3D = vP3Di;bestTriangulated = vbTriangulatedi;}// 如果當前組的good計數小于歷史最優但卻大于歷史次優else if(nGood>secondBestGood){// 說明當前組解是歷史次優點，更新之secondBestGood = nGood;}}// Step 3 選擇最優解。要滿足下面的四個條件// 1. good點數最優解明顯大于次優解，這里取0.75經驗值// 2. 視角差大于規定的閾值// 3. good點數要大于規定的最小的被三角化的點數量// 4. good數要足夠多，達到總數的90%以上if(secondBestGood<0.75*bestGood &&      bestParallax>=minParallax &&bestGood>minTriangulated && bestGood>0.9*N){// 從最佳的解的索引訪問到R，tvR[bestSolutionIdx].copyTo(R21);vt[bestSolutionIdx].copyTo(t21);// 獲得最佳解時，成功三角化的三維點，以后作為初始地圖點使用vP3D = bestP3D;// 獲取特征點的被成功進行三角化的標記vbTriangulated = bestTriangulated;//返回真，找到了最好的解return true;}return false;
}

6.3.3 檢驗分解結果R,t

通過成功三角化的特征點個數判斷分解結果的好壞: 若某特征點的重投影誤差小于4且視差角大于0.36°,則認為該特征點三角化成功

/*** @brief 用位姿來對特征匹配點三角化，從中篩選中合格的三維點* * @param[in] R                                     旋轉矩陣R* @param[in] t                                     平移矩陣t* @param[in] vKeys1                                參考幀特征點  * @param[in] vKeys2                                當前幀特征點* @param[in] vMatches12                            兩幀特征點的匹配關系* @param[in] vbMatchesInliers                      特征點對內點標記* @param[in] K                                     相機內參矩陣* @param[in & out] vP3D                            三角化測量之后的特征點的空間坐標* @param[in] th2                                   重投影誤差的閾值* @param[in & out] vbGood                          標記成功三角化點？* @param[in & out] parallax                        計算出來的比較大的視差角（注意不是最大，具體看后面代碼）* @return int */
int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)
{   // 對給出的特征點對及其R t , 通過三角化檢查解的有效性，也稱為 cheirality check// Calibration parameters//從相機內參數矩陣獲取相機的校正參數const float fx = K.at<float>(0,0);const float fy = K.at<float>(1,1);const float cx = K.at<float>(0,2);const float cy = K.at<float>(1,2);//特征點是否是good點的標記，這里的特征點指的是參考幀中的特征點vbGood = vector<bool>(vKeys1.size(),false);//重設存儲空間坐標的點的大小vP3D.resize(vKeys1.size());//存儲計算出來的每對特征點的視差vector<float> vCosParallax;vCosParallax.reserve(vKeys1.size());// Camera 1 Projection Matrix K[I|0]// Step 1：計算相機的投影矩陣  // 投影矩陣P是一個 3x4 的矩陣，可以將空間中的一個點投影到平面上，獲得其平面坐標，這里均指的是齊次坐標。// 對于第一個相機是 P1=K*[I|0]// 以第一個相機的光心作為世界坐標系, 定義相機的投影矩陣cv::Mat P1(3,4,				//矩陣的大小是3x4CV_32F,			//數據類型是浮點數cv::Scalar(0));	//初始的數值是0//將整個K矩陣拷貝到P1矩陣的左側3x3矩陣，因為 K*I = KK.copyTo(P1.rowRange(0,3).colRange(0,3));// 第一個相機的光心設置為世界坐標系下的原點cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);// Camera 2 Projection Matrix K[R|t]// 計算第二個相機的投影矩陣 P2=K*[R|t]cv::Mat P2(3,4,CV_32F);R.copyTo(P2.rowRange(0,3).colRange(0,3));t.copyTo(P2.rowRange(0,3).col(3));//最終結果是K*[R|t]P2 = K*P2;// 第二個相機的光心在世界坐標系下的坐標cv::Mat O2 = -R.t()*t;//在遍歷開始前，先將good點計數設置為0int nGood=0;// 開始遍歷所有的特征點對for(size_t i=0, iend=vMatches12.size();i<iend;i++){// 跳過outliersif(!vbMatchesInliers[i])continue;// Step 2 獲取特征點對，調用Triangulate() 函數進行三角化，得到三角化測量之后的3D點坐標// kp1和kp2是匹配好的有效特征點const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];//存儲三維點的的坐標cv::Mat p3dC1;// 利用三角法恢復三維點p3dC1Triangulate(kp1,kp2,	//特征點P1,P2,		//投影矩陣p3dC1);		//輸出，三角化測量之后特征點的空間坐標		// Step 3 第一關：檢查三角化的三維點坐標是否合法（非無窮值）// 只要三角測量的結果中有一個是無窮大的就說明三角化失敗，跳過對當前點的處理，進行下一對特征點的遍歷 if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))){//其實這里就算是不這樣寫也沒問題，因為默認的匹配點對就不是good點vbGood[vMatches12[i].first]=false;//繼續對下一對匹配點的處理continue;}// Check parallax// Step 4 第二關：通過三維點深度值正負、兩相機光心視差角大小來檢查是否合法 //得到向量PO1cv::Mat normal1 = p3dC1 - O1;//求取模長，其實就是距離float dist1 = cv::norm(normal1);//同理構造向量PO2cv::Mat normal2 = p3dC1 - O2;//求模長float dist2 = cv::norm(normal2);//根據公式：a.*b=|a||b|cos_theta 可以推導出來下面的式子float cosParallax = normal1.dot(normal2)/(dist1*dist2);// Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)// 如果深度值為負值，為非法三維點跳過該匹配點對// ?視差比較小時，重投影誤差比較大。這里0.99998 對應的角度為0.36°,這里不應該是 cosParallax>0.99998 嗎？// ?因為后面判斷vbGood 點時的條件也是 cosParallax<0.99998 // !可能導致初始化不穩定if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)continue;// Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)// 講空間點p3dC1變換到第2個相機坐標系下變為p3dC2cv::Mat p3dC2 = R*p3dC1+t;	//判斷過程和上面的相同if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)continue;// Step 5 第三關：計算空間點在參考幀和當前幀上的重投影誤差，如果大于閾值則舍棄// Check reprojection error in first image// 計算3D點在第一個圖像上的投影誤差//投影到參考幀圖像上的點的坐標x,yfloat im1x, im1y;//這個使能空間點的z坐標的倒數float invZ1 = 1.0/p3dC1.at<float>(2);//投影到參考幀圖像上。因為參考幀下的相機坐標系和世界坐標系重合，因此這里就直接進行投影就可以了im1x = fx*p3dC1.at<float>(0)*invZ1+cx;im1y = fy*p3dC1.at<float>(1)*invZ1+cy;//參考幀上的重投影誤差，這個的確就是按照定義來的float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);// 重投影誤差太大，跳過淘汰if(squareError1>th2)continue;// Check reprojection error in second image// 計算3D點在第二個圖像上的投影誤差，計算過程和第一個圖像類似float im2x, im2y;// 注意這里的p3dC2已經是第二個相機坐標系下的三維點了float invZ2 = 1.0/p3dC2.at<float>(2);im2x = fx*p3dC2.at<float>(0)*invZ2+cx;im2y = fy*p3dC2.at<float>(1)*invZ2+cy;// 計算重投影誤差float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);// 重投影誤差太大，跳過淘汰if(squareError2>th2)continue;// Step 6 統計經過檢驗的3D點個數，記錄3D點視差角 // 如果運行到這里就說明當前遍歷的這個特征點對靠譜，經過了重重檢驗，說明是一個合格的點，稱之為good點 vCosParallax.push_back(cosParallax);//存儲這個三角化測量后的3D點在世界坐標系下的坐標vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));//good點計數++nGood++;//判斷視差角，只有視差角稍稍大一丟丟的才會給打good點標記//? bug 我覺得這個寫的位置不太對。你的good點計數都++了然后才判斷，不是會讓good點標志和good點計數不一樣嗎if(cosParallax<0.99998)vbGood[vMatches12[i].first]=true;}// Step 7 得到3D點中較小的視差角，并且轉換成為角度制表示if(nGood>0){// 從小到大排序，注意vCosParallax值越大，視差越小sort(vCosParallax.begin(),vCosParallax.end());// !排序后并沒有取最小的視差角，而是取一個較小的視差角// 作者的做法：如果經過檢驗過后的有效3D點小于50個，那么就取最后那個最小的視差角(cos值最大)// 如果大于50個，就取排名第50個的較小的視差角即可，為了避免3D點太多時出現太小的視差角 size_t idx = min(50,int(vCosParallax.size()-1));//將這個選中的角弧度制轉換為角度制parallax = acos(vCosParallax[idx])*180/CV_PI;}else//如果沒有good點那么這個就直接設置為0了parallax=0;//返回good點計數return nGood;
}

SVD求解超定方程

/** 給定投影矩陣P1,P2和圖像上的匹配特征點點kp1,kp2，從而計算三維點坐標* @brief * * @param[in] kp1               特征點, in reference frame* @param[in] kp2               特征點, in current frame* @param[in] P1                投影矩陣P1* @param[in] P2                投影矩陣P2* @param[in & out] x3D         計算的三維點*/
void Initializer::Triangulate(const cv::KeyPoint &kp1,    //特征點, in reference frameconst cv::KeyPoint &kp2,    //特征點, in current frameconst cv::Mat &P1,          //投影矩陣P1const cv::Mat &P2,          //投影矩陣P2cv::Mat &x3D)               //三維點
{// 原理// 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求解，右奇異矩陣的最后一行就是最終的解.//這個就是上面注釋中的矩陣Acv::Mat A(4,4,CV_32F);//構造參數矩陣AA.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);//奇異值分解的結果cv::Mat u,w,vt;//對系數矩陣A進行奇異值分解cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);//根據前面的結論，奇異值分解右矩陣的最后一行其實就是解，原理類似于前面的求最小二乘解，四個未知數四個方程正好正定//別忘了我們更習慣用列向量來表示一個點的空間坐標x3D = vt.row(3).t();//為了符合其次坐標的形式，使最后一維為1x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

6.4 對極幾何

?設點 在相機1、2坐標系下的坐標分別為 、 ,在相機1、2成像平面下的像素坐標分別為 、 ,有:

?6.4.2 極線與極點

?

?

?

?

??

?7. ORB-SLAM2代碼詳解07_跟蹤線程Tracking

?

7.1 各成員函數/變量

Tracking類中定義枚舉類型eTrackingState,用于表示跟蹤狀態,其可能的取值如下

Tracking類的成員變量mState和mLastProcessedState分別表示當前幀的跟蹤狀態和上一幀的跟蹤狀態.?

?

7.1.2 初始化?

?

初始化用于SLAM系統剛開始接收到圖像的幾幀,初始化成功之后就進入正常的跟蹤操作.

Tracking類主函數Tracking::Track()檢查到當前系統的跟蹤狀態mState為NOT_INITIALIZED時,就會進行初始化.

void Tracking::Track() {// ...unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
?// step1. 若還沒初始化,則嘗試初始化if (mState == NOT_INITIALIZED) {if (mSensor == System::STEREO || mSensor == System::RGBD)StereoInitialization();elseMonocularInitialization();if (mState != OK)return;} // ...
}

?7.2 單目相機初始化:?MonocularInitialization()

?單目相機初始化條件: 連續兩幀間成功三角化超過100個點,則初始化成功.

/** @brief 單目的地圖初始化** 并行地計算基礎矩陣和單應性矩陣，選取其中一個模型，恢復出最開始兩幀之間的相對姿態以及點云* 得到初始兩幀的匹配、相對運動、初始MapPoints* * Step 1：（未創建）得到用于初始化的第一幀，初始化需要兩幀* Step 2：（已創建）如果當前幀特征點數大于100，則得到用于單目初始化的第二幀* Step 3：在mInitialFrame與mCurrentFrame中找匹配的特征點對* Step 4：如果初始化的兩幀之間的匹配點太少，重新初始化* Step 5：通過H模型或F模型進行單目初始化，得到兩幀間相對運動、初始MapPoints* Step 6：刪除那些無法進行三角化的匹配點* Step 7：將三角化得到的3D點包裝成MapPoints*/
void Tracking::MonocularInitialization()
{// Step 1 如果單目初始器還沒有被創建，則創建。后面如果重新初始化時會清掉這個if(!mpInitializer){// Set Reference Frame// 單目初始幀的特征點數必須大于100if(mCurrentFrame.mvKeys.size()>100){// 初始化需要兩幀，分別是mInitialFrame，mCurrentFramemInitialFrame = Frame(mCurrentFrame);// 用當前幀更新上一幀mLastFrame = Frame(mCurrentFrame);// mvbPrevMatched  記錄"上一幀"所有特征點mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;// 刪除前判斷一下，來避免出現段錯誤。不過在這里是多余的判斷// 不過在這里是多余的判斷，因為前面已經判斷過了if(mpInitializer)delete mpInitializer;// 由當前幀構造初始器 sigma:1.0 iterations:200mpInitializer =  new Initializer(mCurrentFrame,1.0,200);// 初始化為-1 表示沒有任何匹配。這里面存儲的是匹配的點的idfill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}}else    //如果單目初始化器已經被創建{// Try to initialize// Step 2 如果當前幀特征點數太少（不超過100），則重新構造初始器// NOTICE 只有連續兩幀的特征點個數都大于100時，才能繼續進行初始化過程if((int)mCurrentFrame.mvKeys.size()<=100){delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);fill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}// Find correspondences// Step 3 在mInitialFrame與mCurrentFrame中找匹配的特征點對ORBmatcher matcher(0.9,        //最佳的和次佳特征點評分的比值閾值，這里是比較寬松的，跟蹤時一般是0.7true);      //檢查特征點的方向// 對 mInitialFrame,mCurrentFrame 進行特征點匹配// mvbPrevMatched為參考幀的特征點坐標，初始化存儲的是mInitialFrame中特征點坐標，匹配后存儲的是匹配好的當前幀的特征點坐標// mvIniMatches 保存參考幀F1中特征點是否匹配上，index保存是F1對應特征點索引，值保存的是匹配好的F2特征點索引int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,    //初始化時的參考幀和當前幀mvbPrevMatched,                 //在初始化參考幀中提取得到的特征點mvIniMatches,                   //保存匹配關系100);                           //搜索窗口大小// Check if there are enough correspondences// Step 4 驗證匹配結果，如果初始化的兩幀之間的匹配點太少，重新初始化if(nmatches<100){delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);return;}cv::Mat Rcw; // Current Camera Rotationcv::Mat tcw; // Current Camera Translationvector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)// Step 5 通過H模型或F模型進行單目初始化，得到兩幀間相對運動、初始MapPointsif(mpInitializer->Initialize(mCurrentFrame,      //當前幀mvIniMatches,       //當前幀和參考幀的特征點的匹配關系Rcw, tcw,           //初始化得到的相機的位姿mvIniP3D,           //進行三角化得到的空間點集合vbTriangulated))    //以及對應于mvIniMatches來講,其中哪些點被三角化了{// Step 6 初始化成功后，刪除那些無法進行三角化的匹配點for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++){if(mvIniMatches[i]>=0 && !vbTriangulated[i]){mvIniMatches[i]=-1;nmatches--;}}// Set Frame Poses// Step 7 將初始化的第一幀作為世界坐標系，因此第一幀變換矩陣為單位矩陣mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));// 由Rcw和tcw構造Tcw,并賦值給mTcw，mTcw為世界坐標系到相機坐標系的變換矩陣cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));tcw.copyTo(Tcw.rowRange(0,3).col(3));mCurrentFrame.SetPose(Tcw);// Step 8 創建初始化地圖點MapPoints// Initialize函數會得到mvIniP3D，// mvIniP3D是cv::Point3f類型的一個容器，是個存放3D點的臨時變量，// CreateInitialMapMonocular將3D點包裝成MapPoint類型存入KeyFrame和Map中CreateInitialMapMonocular();}//當初始化成功的時候進行}//如果單目初始化器已經被創建
}

單目初始化成功后調用函數CreateInitialMapMonocular()創建初始化地圖

void Tracking::CreateInitialMapMonocular()
{// Create KeyFrames 認為單目初始化時候的參考幀和當前幀都是關鍵幀KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);  // 第一幀KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);  // 第二幀// Step 1 將初始關鍵幀,當前關鍵幀的描述子轉為BoWpKFini->ComputeBoW();pKFcur->ComputeBoW();// Insert KFs in the map// Step 2 將關鍵幀插入到地圖mpMap->AddKeyFrame(pKFini);mpMap->AddKeyFrame(pKFcur);// Create MapPoints and asscoiate to keyframes// Step 3 用初始化得到的3D點來生成地圖點MapPoints//  mvIniMatches[i] 表示初始化兩幀特征點匹配關系。//  具體解釋：i表示幀1中關鍵點的索引值，vMatches12[i]的值為幀2的關鍵點索引值,沒有匹配關系的話，vMatches12[i]值為 -1for(size_t i=0; i<mvIniMatches.size();i++){// 沒有匹配，跳過if(mvIniMatches[i]<0)continue;//Create MapPoint.// 用三角化點初始化為空間點的世界坐標cv::Mat worldPos(mvIniP3D[i]);// Step 3.1 用3D點構造MapPointMapPoint* pMP = new MapPoint(worldPos,pKFcur, mpMap);// Step 3.2 為該MapPoint添加屬性：// a.觀測到該MapPoint的關鍵幀// b.該MapPoint的描述子// c.該MapPoint的平均觀測方向和深度范圍// 表示該KeyFrame的2D特征點和對應的3D地圖點pKFini->AddMapPoint(pMP,i);pKFcur->AddMapPoint(pMP,mvIniMatches[i]);// a.表示該MapPoint可以被哪個KeyFrame的哪個特征點觀測到pMP->AddObservation(pKFini,i);pMP->AddObservation(pKFcur,mvIniMatches[i]);// b.從眾多觀測到該MapPoint的特征點中挑選最有代表性的描述子pMP->ComputeDistinctiveDescriptors();// c.更新該MapPoint平均觀測方向以及觀測距離的范圍pMP->UpdateNormalAndDepth();//Fill Current Frame structure//mvIniMatches下標i表示在初始化參考幀中的特征點的序號//mvIniMatches[i]是初始化當前幀中的特征點的序號mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;//Add to MapmpMap->AddMapPoint(pMP);}// Update Connections// Step 3.3 更新關鍵幀間的連接關系// 在3D點和關鍵幀之間建立邊，每個邊有一個權重，邊的權重是該關鍵幀與當前幀公共3D點的個數pKFini->UpdateConnections();pKFcur->UpdateConnections();// Bundle Adjustmentcout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;// Step 4 全局BA優化，同時優化所有位姿和三維點Optimizer::GlobalBundleAdjustemnt(mpMap,20);// Set median depth to 1// Step 5 取場景的中值深度，用于尺度歸一化 // 為什么是 pKFini 而不是 pKCur ? 答：都可以的，內部做了位姿變換了float medianDepth = pKFini->ComputeSceneMedianDepth(2);float invMedianDepth = 1.0f/medianDepth;//兩個條件,一個是平均深度要大于0,另外一個是在當前幀中被觀測到的地圖點的數目應該大于100if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100){cout << "Wrong initialization, reseting..." << endl;Reset();return;}// Step 6 將兩幀之間的變換歸一化到平均深度1的尺度下// Scale initial baselinecv::Mat Tc2w = pKFcur->GetPose();// x/z y/z 將z歸一化到1 Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;pKFcur->SetPose(Tc2w);// Scale points// Step 7 把3D點的尺度也歸一化到1// 為什么是pKFini? 是不是就算是使用 pKFcur 得到的結果也是相同的? 答：是的，因為是同樣的三維點vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++){if(vpAllMapPoints[iMP]){MapPoint* pMP = vpAllMapPoints[iMP];pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);}}//  Step 8 將關鍵幀插入局部地圖，更新歸一化后的位姿、局部地圖點mpLocalMapper->InsertKeyFrame(pKFini);mpLocalMapper->InsertKeyFrame(pKFcur);mCurrentFrame.SetPose(pKFcur->GetPose());mnLastKeyFrameId=mCurrentFrame.mnId;mpLastKeyFrame = pKFcur;mvpLocalKeyFrames.push_back(pKFcur);mvpLocalKeyFrames.push_back(pKFini);// 單目初始化之后，得到的初始地圖中的所有點都是局部地圖點mvpLocalMapPoints=mpMap->GetAllMapPoints();mpReferenceKF = pKFcur;//也只能這樣子設置了,畢竟是最近的關鍵幀mCurrentFrame.mpReferenceKF = pKFcur;mLastFrame = Frame(mCurrentFrame);mpMap->SetReferenceMapPoints(mvpLocalMapPoints);mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());mpMap->mvpKeyFrameOrigins.push_back(pKFini);mState=OK;// 初始化成功，至此，初始化過程完成
}

7.3 雙目/RGBD相機初始化:?StereoInitialization()

雙目/RGBD相機的要求就寬松多了,只要左目圖像能找到多于500個特征點,就算是初始化成功.

函數StereoInitialization()內部既完成了初始化,又構建了初始化局部地圖.

/** @brief 雙目和rgbd的地圖初始化，比單目簡單很多** 由于具有深度信息，直接生成MapPoints*/
void Tracking::StereoInitialization()
{// 初始化要求當前幀的特征點超過500if(mCurrentFrame.N>500){// Set Frame pose to the origin// 設定初始位姿為單位旋轉，0平移mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));// Create KeyFrame// 將當前幀構造為初始關鍵幀// mCurrentFrame的數據類型為Frame// KeyFrame包含Frame、地圖3D點、以及BoW// KeyFrame里有一個mpMap，Tracking里有一個mpMap，而KeyFrame里的mpMap都指向Tracking里的這個mpMap// KeyFrame里有一個mpKeyFrameDB，Tracking里有一個mpKeyFrameDB，而KeyFrame里的mpMap都指向Tracking里的這個mpKeyFrameDB// 提問: 為什么要指向Tracking中的相應的變量呢? -- 因為Tracking是主線程，是它創建和加載的這些模塊KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);// Insert KeyFrame in the map// KeyFrame中包含了地圖、反過來地圖中也包含了KeyFrame，相互包含// 在地圖中添加該初始關鍵幀mpMap->AddKeyFrame(pKFini);// Create MapPoints and asscoiate to KeyFrame// 為每個特征點構造MapPointfor(int i=0; i<mCurrentFrame.N;i++){//只有具有正深度的點才會被構造地圖點float z = mCurrentFrame.mvDepth[i];if(z>0){// 通過反投影得到該特征點的世界坐標系下3D坐標cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);// 將3D點構造為MapPointMapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);// 為該MapPoint添加屬性：// a.觀測到該MapPoint的關鍵幀// b.該MapPoint的描述子// c.該MapPoint的平均觀測方向和深度范圍// a.表示該MapPoint可以被哪個KeyFrame的哪個特征點觀測到pNewMP->AddObservation(pKFini,i);// b.從眾多觀測到該MapPoint的特征點中挑選區分度最高的描述子             pNewMP->ComputeDistinctiveDescriptors();// c.更新該MapPoint平均觀測方向以及觀測距離的范圍pNewMP->UpdateNormalAndDepth();// 在地圖中添加該MapPointmpMap->AddMapPoint(pNewMP);// 表示該KeyFrame的哪個特征點可以觀測到哪個3D點pKFini->AddMapPoint(pNewMP,i);// 將該MapPoint添加到當前幀的mvpMapPoints中// 為當前Frame的特征點與MapPoint之間建立索引mCurrentFrame.mvpMapPoints[i]=pNewMP;}}cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;// 在局部地圖中添加該初始關鍵幀mpLocalMapper->InsertKeyFrame(pKFini);// 更新當前幀為上一幀mLastFrame = Frame(mCurrentFrame);mnLastKeyFrameId=mCurrentFrame.mnId;mpLastKeyFrame = pKFini;mvpLocalKeyFrames.push_back(pKFini);//? 這個局部地圖點竟然..不在mpLocalMapper中管理?// 我現在的想法是，這個點只是暫時被保存在了 Tracking 線程之中， 所以稱之為 local // 初始化之后，通過雙目圖像生成的地圖點，都應該被認為是局部地圖點mvpLocalMapPoints=mpMap->GetAllMapPoints();mpReferenceKF = pKFini;mCurrentFrame.mpReferenceKF = pKFini;// 把當前（最新的）局部MapPoints作為ReferenceMapPoints// ReferenceMapPoints是DrawMapPoints函數畫圖的時候用的mpMap->SetReferenceMapPoints(mvpLocalMapPoints);mpMap->mvpKeyFrameOrigins.push_back(pKFini);mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);//追蹤成功mState=OK;}
}

7.4 初始位姿估計

當Tracking線程接收到一幀圖像后,會先估計其初始位姿,再根據估計出的初始位姿跟蹤局部地圖并進一步優化位姿.

初始位姿估計有三種手段:

根據恒速運動模型估計位姿TrackWithMotionModel()
根據參考幀估計位姿TrackReferenceKeyFrame()
通過重定位估計位姿Relocalization()

void Tracking::Track() {// ...unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
?// step1. 若還沒初始化,則嘗試初始化if (mState == NOT_INITIALIZED) {// 初始化} else {// step2. 若系統已初始化,就進行跟蹤(或重定位)bool bOK;
?// step2.1. 符合條件時,優先根據運動模型跟蹤,如運動模型跟蹤失敗,就根據參考幀進行跟蹤if (mState == OK) {if (mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId + 2) {     // 判斷當前關鍵幀是否具有較穩定的速度bOK = TrackReferenceKeyFrame();} else {bOK = TrackWithMotionModel();if (!bOK)bOK = TrackReferenceKeyFrame();}} else {// step2.2. 若上一幀沒跟蹤丟失,則這一幀重定位bOK = Relocalization();}// ...if (bOK)mState = OK;elsemState = LOST;}// ...
}

7.5 根據恒速運動模型估計初始位姿: TrackWithMotionModel()

恒速運動模型假定連續幾幀間的運動速度是恒定的;基于此假設,根據運動速度mVelocity和上一幀的位姿mLastFrame.mTcw計算出本幀位姿的估計值,再進行位姿優化.

成員變量mVelocity保存前一幀的速度,主函數Tracking::Track()中調用完函數Tracking::TrackLocalMap()更新局部地圖和當前幀位姿后,就計算速度并賦值給mVelocity.

/*** @brief 根據恒定速度模型用上一幀地圖點來對當前幀進行跟蹤* Step 1：更新上一幀的位姿；對于雙目或RGB-D相機，還會根據深度值生成臨時地圖點* Step 2：根據上一幀特征點對應地圖點進行投影匹配* Step 3：優化當前幀位姿* Step 4：剔除地圖點中外點* @return 如果匹配數大于10，認為跟蹤成功，返回true*/
bool Tracking::TrackWithMotionModel()
{// 最小距離 < 0.9*次小距離 匹配成功，檢查旋轉ORBmatcher matcher(0.9,true);// Update last frame pose according to its reference keyframe// Create "visual odometry" points// Step 1：更新上一幀的位姿；對于雙目或RGB-D相機，還會根據深度值生成臨時地圖點UpdateLastFrame();// Step 2：根據之前估計的速度，用恒速模型得到當前幀的初始位姿。mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);// 清空當前幀的地圖點fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));// Project points seen in previous frame// 設置特征匹配過程中的搜索半徑int th;if(mSensor!=System::STEREO)th=15;//單目elseth=7;//雙目// Step 3：用上一幀地圖點進行投影匹配，如果匹配點不夠，則擴大搜索半徑再來一次int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);// If few matches, uses a wider window search// 如果匹配點太少，則擴大搜索半徑再來一次if(nmatches<20){fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th}// 如果還是不能夠獲得足夠的匹配點,那么就認為跟蹤失敗if(nmatches<20)return false;// Optimize frame pose with all matches// Step 4：利用3D-2D投影關系，優化當前幀位姿Optimizer::PoseOptimization(&mCurrentFrame);// Discard outliers// Step 5：剔除地圖點中外點int nmatchesMap = 0;for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){if(mCurrentFrame.mvbOutlier[i]){// 如果優化后判斷某個地圖點是外點，清除它的所有關系MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)// 累加成功匹配到的地圖點數目nmatchesMap++;}}    if(mbOnlyTracking){// 純定位模式下：如果成功追蹤的地圖點非常少,那么這里的mbVO標志就會置位mbVO = nmatchesMap<10;return nmatches>20;}// Step 6：匹配超過10個點就認為跟蹤成功return nmatchesMap>=10;
}

?為保證位姿估計的準確性,對于雙目/RGBD相機,為前一幀生成臨時地圖點.

/*** @brief 更新上一幀位姿，在上一幀中生成臨時地圖點* 單目情況：只計算了上一幀的世界坐標系位姿* 雙目和rgbd情況：選取有有深度值的并且沒有被選為地圖點的點生成新的臨時地圖點，提高跟蹤魯棒性*/
void Tracking::UpdateLastFrame()
{// Update pose according to reference keyframe// Step 1：利用參考關鍵幀更新上一幀在世界坐標系下的位姿// 上一普通幀的參考關鍵幀，注意這里用的是參考關鍵幀（位姿準）而不是上上一幀的普通幀KeyFrame* pRef = mLastFrame.mpReferenceKF;  // ref_keyframe 到 lastframe的位姿變換cv::Mat Tlr = mlRelativeFramePoses.back();// 將上一幀的世界坐標系下的位姿計算出來// l:last, r:reference, w:world// Tlw = Tlr*Trw mLastFrame.SetPose(Tlr*pRef->GetPose()); // 如果上一幀為關鍵幀，或者單目的情況，則退出if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)return;// Step 2：對于雙目或rgbd相機，為上一幀生成新的臨時地圖點// 注意這些地圖點只是用來跟蹤，不加入到地圖中，跟蹤完后會刪除// Create "visual odometry" MapPoints// We sort points according to their measured depth by the stereo/RGB-D sensor// Step 2.1：得到上一幀中具有有效深度值的特征點（不一定是地圖點）vector<pair<float,int> > vDepthIdx;vDepthIdx.reserve(mLastFrame.N);for(int i=0; i<mLastFrame.N;i++){float z = mLastFrame.mvDepth[i];if(z>0){// vDepthIdx第一個元素是某個點的深度,第二個元素是對應的特征點idvDepthIdx.push_back(make_pair(z,i));}}// 如果上一幀中沒有有效深度的點,那么就直接退出if(vDepthIdx.empty())return;// 按照深度從小到大排序sort(vDepthIdx.begin(),vDepthIdx.end());// We insert all close points (depth<mThDepth)// If less than 100 close points, we insert the 100 closest ones.// Step 2.2：從中找出不是地圖點的部分  int nPoints = 0;for(size_t j=0; j<vDepthIdx.size();j++){int i = vDepthIdx[j].second;bool bCreateNew = false;// 如果這個點對應在上一幀中的地圖點沒有,或者創建后就沒有被觀測到,那么就生成一個臨時的地圖點MapPoint* pMP = mLastFrame.mvpMapPoints[i];if(!pMP)bCreateNew = true;else if(pMP->Observations()<1)      {// 地圖點被創建后就沒有被觀測，認為不靠譜，也需要重新創建bCreateNew = true;}if(bCreateNew){// Step 2.3：需要創建的點，包裝為地圖點。只是為了提高雙目和RGBD的跟蹤成功率，并沒有添加復雜屬性，因為后面會扔掉// 反投影到世界坐標系中cv::Mat x3D = mLastFrame.UnprojectStereo(i);MapPoint* pNewMP = new MapPoint(x3D,            // 世界坐標系坐標mpMap,          // 跟蹤的全局地圖&mLastFrame,    // 存在這個特征點的幀(上一幀)i);             // 特征點id// 加入上一幀的地圖點中mLastFrame.mvpMapPoints[i]=pNewMP; // 標記為臨時添加的MapPoint，之后在CreateNewKeyFrame之前會全部刪除mlpTemporalPoints.push_back(pNewMP);nPoints++;}else{// 因為從近到遠排序，記錄其中不需要創建地圖點的個數nPoints++;}// Step 2.4：如果地圖點質量不好，停止創建地圖點// 停止新增臨時地圖點必須同時滿足以下條件：// 1、當前的點的深度已經超過了設定的深度閾值（35倍基線）// 2、nPoints已經超過100個點，說明距離比較遠了，可能不準確，停掉退出if(vDepthIdx[j].first>mThDepth && nPoints>100)break;}
}

7.6 根據參考幀估計位姿: TrackReferenceKeyFrame()

成員變量mpReferenceKF保存Tracking線程當前的參考關鍵幀,參考關鍵幀有兩個來源:

每當Tracking線程創建一個新的參考關鍵幀時,就將其設為參考關鍵幀.
跟蹤局部地圖的函數Tracking::TrackLocalMap()內部會將與當前幀共視點最多的局部關鍵幀設為參考關鍵幀.

/** @brief 用參考關鍵幀的地圖點來對當前普通幀進行跟蹤* * Step 1：將當前普通幀的描述子轉化為BoW向量* Step 2：通過詞袋BoW加速當前幀與參考幀之間的特征點匹配* Step 3: 將上一幀的位姿態作為當前幀位姿的初始值* Step 4: 通過優化3D-2D的重投影誤差來獲得位姿* Step 5：剔除優化后的匹配點中的外點* @return 如果匹配數超10，返回true* */
bool Tracking::TrackReferenceKeyFrame()
{// Compute Bag of Words vector// Step 1：將當前幀的描述子轉化為BoW向量mCurrentFrame.ComputeBoW();// We perform first an ORB matching with the reference keyframe// If enough matches are found we setup a PnP solverORBmatcher matcher(0.7,true);vector<MapPoint*> vpMapPointMatches;// Step 2：通過詞袋BoW加速當前幀與參考幀之間的特征點匹配int nmatches = matcher.SearchByBoW(mpReferenceKF,          //參考關鍵幀mCurrentFrame,          //當前幀vpMapPointMatches);     //存儲匹配關系// 匹配數目小于15，認為跟蹤失敗if(nmatches<15)return false;// Step 3:將上一幀的位姿態作為當前幀位姿的初始值mCurrentFrame.mvpMapPoints = vpMapPointMatches;mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw設置初值，在PoseOptimization可以收斂快一些// Step 4:通過優化3D-2D的重投影誤差來獲得位姿Optimizer::PoseOptimization(&mCurrentFrame);// Discard outliers// Step 5：剔除優化后的匹配點中的外點//之所以在優化之后才剔除外點，是因為在優化的過程中就有了對這些外點的標記int nmatchesMap = 0;for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){//如果對應到的某個特征點是外點if(mCurrentFrame.mvbOutlier[i]){//清除它在當前幀中存在過的痕跡MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)//匹配的內點計數++nmatchesMap++;}}// 跟蹤成功的數目超過10才認為跟蹤成功，否則跟蹤失敗return nmatchesMap>=10;
}

思考: 為什么函數Tracking::TrackReferenceKeyFrame()沒有檢查當前參考幀mpReferenceKF是否被LocalMapping線程刪除了?

回答: 因為LocalMapping線程剔除冗余關鍵幀函數LocalMapping::KeyFrameCulling()不會刪除最新的參考幀,有可能被刪除的都是之前的參考幀.

7.7 通過重定位估計位姿:?Relocalization()

當TrackWithMotionModel()和TrackReferenceKeyFrame()都失敗后,就會調用函數Relocalization()通過重定位估計位姿.

/*** @details 重定位過程* @return true * @return false * * Step 1：計算當前幀特征點的詞袋向量* Step 2：找到與當前幀相似的候選關鍵幀* Step 3：通過BoW進行匹配* Step 4：通過EPnP算法估計姿態* Step 5：通過PoseOptimization對姿態進行優化求解* Step 6：如果內點較少，則通過投影的方式對之前未匹配的點進行匹配，再進行優化求解*/
bool Tracking::Relocalization()
{// Compute Bag of Words Vector// Step 1：計算當前幀特征點的詞袋向量mCurrentFrame.ComputeBoW();// Relocalization is performed when tracking is lost// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation// Step 2：用詞袋找到與當前幀相似的候選關鍵幀vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);// 如果沒有候選關鍵幀，則退出if(vpCandidateKFs.empty())return false;const int nKFs = vpCandidateKFs.size();// We perform first an ORB matching with each candidate// If enough matches are found we setup a PnP solverORBmatcher matcher(0.75,true);//每個關鍵幀的解算器vector<PnPsolver*> vpPnPsolvers;vpPnPsolvers.resize(nKFs);//每個關鍵幀和當前幀中特征點的匹配關系vector<vector<MapPoint*> > vvpMapPointMatches;vvpMapPointMatches.resize(nKFs);//放棄某個關鍵幀的標記vector<bool> vbDiscarded;vbDiscarded.resize(nKFs);//有效的候選關鍵幀數目int nCandidates=0;// Step 3：遍歷所有的候選關鍵幀，通過詞袋進行快速匹配，用匹配結果初始化PnP Solverfor(int i=0; i<nKFs; i++){KeyFrame* pKF = vpCandidateKFs[i];if(pKF->isBad())vbDiscarded[i] = true;else{// 當前幀和候選關鍵幀用BoW進行快速匹配，匹配結果記錄在vvpMapPointMatches，nmatches表示匹配的數目int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);// 如果和當前幀的匹配數小于15,那么只能放棄這個關鍵幀if(nmatches<15){vbDiscarded[i] = true;continue;}else{// 如果匹配數目夠用，用匹配結果初始化EPnPsolver// 為什么用EPnP? 因為計算復雜度低，精度高PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);pSolver->SetRansacParameters(0.99,   //用于計算RANSAC迭代次數理論值的概率10,     //最小內點數, 但是要注意在程序中實際上是min(給定最小內點數,最小集,內點數理論值),不一定使用這個300,    //最大迭代次數4,      //最小集(求解這個問題在一次采樣中所需要采樣的最少的點的個數,對于Sim3是3,EPnP是4),參與到最小內點數的確定過程中0.5,    //這個是表示(最小內點數/樣本總數);實際上的RANSAC正常退出的時候所需要的最小內點數其實是根據這個量來計算得到的5.991); // 自由度為2的卡方檢驗的閾值,程序中還會根據特征點所在的圖層對這個閾值進行縮放vpPnPsolvers[i] = pSolver;nCandidates++;}}}// Alternatively perform some iterations of P4P RANSAC// Until we found a camera pose supported by enough inliers// 這里的 P4P RANSAC是Epnp，每次迭代需要4個點// 是否已經找到相匹配的關鍵幀的標志bool bMatch = false;ORBmatcher matcher2(0.9,true);// Step 4: 通過一系列操作,直到找到能夠匹配上的關鍵幀// 為什么搞這么復雜？答：是擔心誤閉環while(nCandidates>0 && !bMatch){//遍歷當前所有的候選關鍵幀for(int i=0; i<nKFs; i++){// 忽略放棄的if(vbDiscarded[i])continue;//內點標記vector<bool> vbInliers;     //內點數int nInliers;// 表示RANSAC已經沒有更多的迭代次數可用 -- 也就是說數據不夠好，RANSAC也已經盡力了。。。bool bNoMore;// Step 4.1：通過EPnP算法估計姿態，迭代5次PnPsolver* pSolver = vpPnPsolvers[i];cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);// If Ransac reachs max. iterations discard keyframe// bNoMore 為true 表示已經超過了RANSAC最大迭代次數，就放棄當前關鍵幀if(bNoMore){vbDiscarded[i]=true;nCandidates--;}// If a Camera Pose is computed, optimizeif(!Tcw.empty()){//  Step 4.2：如果EPnP 計算出了位姿，對內點進行BA優化Tcw.copyTo(mCurrentFrame.mTcw);// EPnP 里RANSAC后的內點的集合set<MapPoint*> sFound;const int np = vbInliers.size();//遍歷所有內點for(int j=0; j<np; j++){if(vbInliers[j]){mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];sFound.insert(vvpMapPointMatches[i][j]);}elsemCurrentFrame.mvpMapPoints[j]=NULL;}// 只優化位姿,不優化地圖點的坐標，返回的是內點的數量int nGood = Optimizer::PoseOptimization(&mCurrentFrame);// 如果優化之后的內點數目不多，跳過了當前候選關鍵幀,但是卻沒有放棄當前幀的重定位if(nGood<10)continue;// 刪除外點對應的地圖點for(int io =0; io<mCurrentFrame.N; io++)if(mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);// If few inliers, search by projection in a coarse window and optimize again// Step 4.3：如果內點較少，則通過投影的方式對之前未匹配的點進行匹配，再進行優化求解// 前面的匹配關系是用詞袋匹配過程得到的if(nGood<50){// 通過投影的方式將關鍵幀中未匹配的地圖點投影到當前幀中, 生成新的匹配int nadditional = matcher2.SearchByProjection(mCurrentFrame,          //當前幀vpCandidateKFs[i],      //關鍵幀sFound,                 //已經找到的地圖點集合，不會用于PNP10,                     //窗口閾值，會乘以金字塔尺度100);                   //匹配的ORB描述子距離應該小于這個閾值// 如果通過投影過程新增了比較多的匹配特征點對if(nadditional+nGood>=50){// 根據投影匹配的結果，再次采用3D-2D pnp BA優化位姿nGood = Optimizer::PoseOptimization(&mCurrentFrame);// If many inliers but still not enough, search by projection again in a narrower window// the camera has been already optimized with many points// Step 4.4：如果BA后內點數還是比較少(<50)但是還不至于太少(>30)，可以挽救一下, 最后垂死掙扎 // 重新執行上一步 4.3的過程，只不過使用更小的搜索窗口// 這里的位姿已經使用了更多的點進行了優化,應該更準，所以使用更小的窗口搜索if(nGood>30 && nGood<50){// 用更小窗口、更嚴格的描述子閾值，重新進行投影搜索匹配sFound.clear();for(int ip =0; ip<mCurrentFrame.N; ip++)if(mCurrentFrame.mvpMapPoints[ip])sFound.insert(mCurrentFrame.mvpMapPoints[ip]);nadditional =matcher2.SearchByProjection(mCurrentFrame,          //當前幀vpCandidateKFs[i],      //候選的關鍵幀sFound,                 //已經找到的地圖點，不會用于PNP3,                      //新的窗口閾值，會乘以金字塔尺度64);                    //匹配的ORB描述子距離應該小于這個閾值// Final optimization// 如果成功挽救回來，匹配數目達到要求，最后BA優化一下if(nGood+nadditional>=50){nGood = Optimizer::PoseOptimization(&mCurrentFrame);//更新地圖點for(int io =0; io<mCurrentFrame.N; io++)if(mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io]=NULL;}//如果還是不能夠滿足就放棄了}}}// If the pose is supported by enough inliers stop ransacs and continue// 如果對于當前的候選關鍵幀已經有足夠的內點(50個)了,那么就認為重定位成功if(nGood>=50){bMatch = true;// 只要有一個候選關鍵幀重定位成功，就退出循環，不考慮其他候選關鍵幀了break;}}}//一直運行,知道已經沒有足夠的關鍵幀,或者是已經有成功匹配上的關鍵幀}// 折騰了這么久還是沒有匹配上，重定位失敗if(!bMatch){return false;}else{// 如果匹配上了,說明當前幀重定位成功了(當前幀已經有了自己的位姿)// 記錄成功重定位幀的id，防止短時間多次重定位mnLastRelocFrameId = mCurrentFrame.mnId;return true;}
}

7.8 跟蹤局部地圖:?TrackLocalMap()

?

成功估計當前幀的初始位姿后,基于當前位姿更新局部地圖并優化當前幀位姿,主要流程:

更新局部地圖,包括局部關鍵幀列表mvpLocalKeyFrames和局部地圖點列表mvpLocalMapPoints.

將局部地圖點投影到當前幀特征點上.

進行位姿BA,優化當前幀位姿.

更新地圖點觀測數值,統計內點個數.

這里的地圖點觀測數值會被用作LocalMapping線程中LocalMapping::MapPointCulling()函數剔除壞點的標準之一.

根據內點數判斷是否跟蹤成功.

跟蹤局部地圖,優化當前幀位姿
TrackLocalMap()更新局部地圖
UpdateLocalMap()將局部地圖點投影到當前幀特征點上
SearchLocalPoints()對當前幀位姿進行BA優化更新地圖點觀測根據內點數判斷是否跟蹤成功更新局部關鍵幀
UpdateLocalKeyFrames()更新局部地圖點
UpdateLocalPoints()

/*** @brief 用局部地圖進行跟蹤，進一步優化位姿* * 1. 更新局部地圖，包括局部關鍵幀和關鍵點* 2. 對局部MapPoints進行投影匹配* 3. 根據匹配對估計當前幀的姿態* 4. 根據姿態剔除誤匹配* @return true if success* * Step 1：更新局部關鍵幀mvpLocalKeyFrames和局部地圖點mvpLocalMapPoints * Step 2：在局部地圖中查找與當前幀匹配的MapPoints, 其實也就是對局部地圖點進行跟蹤* Step 3：更新局部所有MapPoints后對位姿再次優化* Step 4：更新當前幀的MapPoints被觀測程度，并統計跟蹤局部地圖的效果* Step 5：決定是否跟蹤成功*/
bool Tracking::TrackLocalMap()
{// We have an estimation of the camera pose and some map points tracked in the frame.// We retrieve the local map and try to find matches to points in the local map.// Update Local KeyFrames and Local Points// Step 1：更新局部關鍵幀 mvpLocalKeyFrames 和局部地圖點 mvpLocalMapPointsUpdateLocalMap();// Step 2：篩選局部地圖中新增的在視野范圍內的地圖點，投影到當前幀搜索匹配，得到更多的匹配關系SearchLocalPoints();// Optimize Pose// 在這個函數之前，在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿優化，// Step 3：前面新增了更多的匹配關系，BA優化得到更準確的位姿Optimizer::PoseOptimization(&mCurrentFrame);mnMatchesInliers = 0;// Update MapPoints Statistics// Step 4：更新當前幀的地圖點被觀測程度，并統計跟蹤局部地圖后匹配數目for(int i=0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){// 由于當前幀的地圖點可以被當前幀觀測到，其被觀測統計量加1if(!mCurrentFrame.mvbOutlier[i]){// 找到該點的幀數mnFound 加 1mCurrentFrame.mvpMapPoints[i]->IncreaseFound();//查看當前是否是在純定位過程if(!mbOnlyTracking){// 如果該地圖點被相機觀測數目nObs大于0，匹配內點計數+1// nObs： 被觀測到的相機數目，單目+1，雙目或RGB-D則+2if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)mnMatchesInliers++;}else// 記錄當前幀跟蹤到的地圖點數目，用于統計跟蹤效果mnMatchesInliers++;}// 如果這個地圖點是外點,并且當前相機輸入還是雙目的時候,就刪除這個點// ?單目就不管嗎else if(mSensor==System::STEREO)  mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);}}// Decide if the tracking was succesful// More restrictive if there was a relocalization recently// Step 5：根據跟蹤匹配數目及重定位情況決定是否跟蹤成功// 如果最近剛剛發生了重定位,那么至少成功匹配50個點才認為是成功跟蹤if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)return false;//如果是正常的狀態話只要跟蹤的地圖點大于30個就認為成功了if(mnMatchesInliers<30)return false;elsereturn true;
}

函數Tracking::UpdateLocalMap()依次調用函數Tracking::UpdateLocalKeyFrames()更新局部關鍵幀列表mvpLocalKeyFrames和函數Tracking::UpdateLocalPoints()更新局部地圖點列表mvpLocalMapPoints.

/*** @brief 更新LocalMap** 局部地圖包括： * 1、K1個關鍵幀、K2個臨近關鍵幀和參考關鍵幀* 2、由這些關鍵幀觀測到的MapPoints*/
void Tracking::UpdateLocalMap()
{// This is for visualization// 設置參考地圖點用于繪圖顯示局部地圖點（紅色）mpMap->SetReferenceMapPoints(mvpLocalMapPoints);// Update// 用共視圖來更新局部關鍵幀和局部地圖點UpdateLocalKeyFrames();UpdateLocalPoints();
}

函數Tracking::UpdateLocalKeyFrames()內,局部關鍵幀列表mvpLocalKeyFrames會被清空并重新賦值,包括以下3部分:

當前地圖點的所有共視關鍵幀.
1中所有關鍵幀的父子關鍵幀.
1中所有關鍵幀共視關系前10大的共視關鍵幀.
更新完局部關鍵幀列表mvpLocalKeyFrames后,還將與當前幀共視關系最強的關鍵幀設為參考關鍵幀mpReferenceKF.

函數Tracking::UpdateLocalPoints()內,局部地圖點列表mvpLocalMapPoints會被清空并賦值為局部關鍵幀列表mvpLocalKeyFrames的所有地圖點.

/*** @brief 用局部地圖點進行投影匹配，得到更多的匹配關系* 注意：局部地圖點中已經是當前幀地圖點的不需要再投影，只需要將此外的并且在視野范圍內的點和當前幀進行投影匹配*/
void Tracking::SearchLocalPoints()
{// Do not search map points already matched// Step 1：遍歷當前幀的地圖點，標記這些地圖點不參與之后的投影搜索匹配for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++){MapPoint* pMP = *vit;if(pMP){if(pMP->isBad()){*vit = static_cast<MapPoint*>(NULL);}else{// 更新能觀測到該點的幀數加1(被當前幀觀測了)pMP->IncreaseVisible();// 標記該點被當前幀觀測到pMP->mnLastFrameSeen = mCurrentFrame.mnId;// 標記該點在后面搜索匹配時不被投影，因為已經有匹配了pMP->mbTrackInView = false;}}}// 準備進行投影匹配的點的數目int nToMatch=0;// Project points in frame and check its visibility// Step 2：判斷所有局部地圖點中除當前幀地圖點外的點，是否在當前幀視野范圍內for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++){MapPoint* pMP = *vit;// 已經被當前幀觀測到的地圖點肯定在視野范圍內，跳過if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)continue;// 跳過壞點if(pMP->isBad())continue;// Project (this fills MapPoint variables for matching)// 判斷地圖點是否在在當前幀視野內if(mCurrentFrame.isInFrustum(pMP,0.5)){// 觀測到該點的幀數加1pMP->IncreaseVisible();// 只有在視野范圍內的地圖點才參與之后的投影匹配nToMatch++;}}// Step 3：如果需要進行投影匹配的點的數目大于0，就進行投影匹配，增加更多的匹配關系if(nToMatch>0){ORBmatcher matcher(0.8);int th = 1;if(mSensor==System::RGBD)   //RGBD相機輸入的時候,搜索的閾值會變得稍微大一些th=3;// If the camera has been relocalised recently, perform a coarser search// 如果不久前進行過重定位，那么進行一個更加寬泛的搜索，閾值需要增大if(mCurrentFrame.mnId<mnLastRelocFrameId+2)th=5;// 投影匹配得到更多的匹配關系matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);}
}

7.9 關鍵幀的創建

?7.9.1 判斷是否需要創建新關鍵幀:?NeedNewKeyFrame()

是否生成關鍵幀,需要考慮以下幾個方面:

最近是否進行過重定位,重定位后位姿不會太準,不適合做參考幀.
當前系統的工作狀態: 如果LocalMapping線程還有很多KeyFrame沒處理的話,不適合再給它增加負擔了.
距離上次創建關鍵幀經過的時間: 如果很長時間沒創建關鍵幀了的話,就要抓緊創建關鍵幀了.
當前幀的質量: 當前幀觀測到的地圖點要足夠多,同時與參考關鍵幀的重合程度不能太大.
?

總體而言,ORB-SLAM2插入關鍵幀的策略還是比較寬松的,因為后面LocalMapping線程的函數LocalMapping::KeyFrameCulling()會剔除冗余關鍵幀,因此在系統處理得過來的情況下,要盡量多創建關鍵幀.

?

7.9.2 創建新關鍵幀:?CreateNewKeyFrame()

創建新關鍵幀時,對于雙目/RGBD相機輸入情況下也創建新地圖點.

/*** @brief 創建新的關鍵幀* 對于非單目的情況，同時創建新的MapPoints* * Step 1：將當前幀構造成關鍵幀* Step 2：將當前關鍵幀設置為當前幀的參考關鍵幀* Step 3：對于雙目或rgbd攝像頭，為當前幀生成新的MapPoints*/
void Tracking::CreateNewKeyFrame()
{// 如果局部建圖線程關閉了,就無法插入關鍵幀if(!mpLocalMapper->SetNotStop(true))return;// Step 1：將當前幀構造成關鍵幀KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);// Step 2：將當前關鍵幀設置為當前幀的參考關鍵幀// 在UpdateLocalKeyFrames函數中會將與當前關鍵幀共視程度最高的關鍵幀設定為當前幀的參考關鍵幀mpReferenceKF = pKF;mCurrentFrame.mpReferenceKF = pKF;// 這段代碼和 Tracking::UpdateLastFrame 中的那一部分代碼功能相同// Step 3：對于雙目或rgbd攝像頭，為當前幀生成新的地圖點；單目無操作if(mSensor!=System::MONOCULAR){// 根據Tcw計算mRcw、mtcw和mRwc、mOwmCurrentFrame.UpdatePoseMatrices();// We sort points by the measured depth by the stereo/RGBD sensor.// We create all those MapPoints whose depth < mThDepth.// If there are less than 100 close points we create the 100 closest.// Step 3.1：得到當前幀有深度值的特征點（不一定是地圖點）vector<pair<float,int> > vDepthIdx;vDepthIdx.reserve(mCurrentFrame.N);for(int i=0; i<mCurrentFrame.N; i++){float z = mCurrentFrame.mvDepth[i];if(z>0){// 第一個元素是深度,第二個元素是對應的特征點的idvDepthIdx.push_back(make_pair(z,i));}}if(!vDepthIdx.empty()){// Step 3.2：按照深度從小到大排序sort(vDepthIdx.begin(),vDepthIdx.end());// Step 3.3：從中找出不是地圖點的生成臨時地圖點 // 處理的近點的個數int nPoints = 0;for(size_t j=0; j<vDepthIdx.size();j++){int i = vDepthIdx[j].second;bool bCreateNew = false;// 如果這個點對應在上一幀中的地圖點沒有,或者創建后就沒有被觀測到,那么就生成一個臨時的地圖點MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(!pMP)bCreateNew = true;else if(pMP->Observations()<1){bCreateNew = true;mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);}// 如果需要就新建地圖點，這里的地圖點不是臨時的，是全局地圖中新建地圖點，用于跟蹤if(bCreateNew){cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);// 這些添加屬性的操作是每次創建MapPoint后都要做的pNewMP->AddObservation(pKF,i);pKF->AddMapPoint(pNewMP,i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);mCurrentFrame.mvpMapPoints[i]=pNewMP;nPoints++;}else{// 因為從近到遠排序，記錄其中不需要創建地圖點的個數nPoints++;}// Step 3.4：停止新建地圖點必須同時滿足以下條件：// 1、當前的點的深度已經超過了設定的深度閾值（35倍基線）// 2、nPoints已經超過100個點，說明距離比較遠了，可能不準確，停掉退出if(vDepthIdx[j].first>mThDepth && nPoints>100)break;}}}// Step 4：插入關鍵幀// 關鍵幀插入到列表 mlNewKeyFrames中，等待local mapping線程臨幸mpLocalMapper->InsertKeyFrame(pKF);// 插入好了，允許局部建圖停止mpLocalMapper->SetNotStop(false);// 當前幀成為新的關鍵幀，更新mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF;
}

7.10 跟蹤函數:?Track()

主要關注成員變量mState的變化:

void Tracking::Track()
{// track包含兩部分：估計運動、跟蹤局部地圖// mState為tracking的狀態，包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST// 如果圖像復位過、或者第一次運行，則為NO_IMAGE_YET狀態if(mState==NO_IMAGES_YET){mState = NOT_INITIALIZED;}// mLastProcessedState 存儲了Tracking最新的狀態，用于FrameDrawer中的繪制mLastProcessedState=mState;// Get Map Mutex -> Map cannot be changed// 地圖更新時加鎖。保證地圖不會發生變化// 疑問:這樣子會不會影響地圖的實時更新?// 回答：主要耗時在構造幀中特征點的提取和匹配部分,在那個時候地圖是沒有被上鎖的,有足夠的時間更新地圖unique_lock<mutex> lock(mpMap->mMutexMapUpdate);// Step 1：地圖初始化if(mState==NOT_INITIALIZED){if(mSensor==System::STEREO || mSensor==System::RGBD)//雙目RGBD相機的初始化共用一個函數StereoInitialization();else//單目初始化MonocularInitialization();//更新幀繪制器中存儲的最新狀態mpFrameDrawer->Update(this);//這個狀態量在上面的初始化函數中被更新if(mState!=OK)return;}else{// System is initialized. Track Frame.// bOK為臨時變量，用于表示每個函數是否執行成功bool bOK;// Initial camera pose estimation using motion model or relocalization (if tracking is lost)// mbOnlyTracking等于false表示正常SLAM模式（定位+地圖更新），mbOnlyTracking等于true表示僅定位模式// tracking 類構造時默認為false。在viewer中有個開關ActivateLocalizationMode，可以控制是否開啟mbOnlyTrackingif(!mbOnlyTracking){// Local Mapping is activated. This is the normal behaviour, unless// you explicitly activate the "only tracking" mode.// Step 2：跟蹤進入正常SLAM模式，有地圖更新// 是否正常跟蹤if(mState==OK){// Local Mapping might have changed some MapPoints tracked in last frame// Step 2.1 檢查并更新上一幀被替換的MapPoints// 局部建圖線程則可能會對原有的地圖點進行替換.在這里進行檢查CheckReplacedInLastFrame();// Step 2.2 運動模型是空的或剛完成重定位，跟蹤參考關鍵幀；否則恒速模型跟蹤// 第一個條件,如果運動模型為空,說明是剛初始化開始，或者已經跟丟了// 第二個條件,如果當前幀緊緊地跟著在重定位的幀的后面，我們將重定位幀來恢復位姿// mnLastRelocFrameId 上一次重定位的那一幀if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2){// 用最近的關鍵幀來跟蹤當前的普通幀// 通過BoW的方式在參考幀中找當前幀特征點的匹配點// 優化每個特征點都對應3D點重投影誤差即可得到位姿bOK = TrackReferenceKeyFrame();}else{// 用最近的普通幀來跟蹤當前的普通幀// 根據恒速模型設定當前幀的初始位姿// 通過投影的方式在參考幀中找當前幀特征點的匹配點// 優化每個特征點所對應3D點的投影誤差即可得到位姿bOK = TrackWithMotionModel();if(!bOK)//根據恒速模型失敗了，只能根據參考關鍵幀來跟蹤bOK = TrackReferenceKeyFrame();}}else{// 如果跟蹤狀態不成功,那么就只能重定位了// BOW搜索，EPnP求解位姿bOK = Relocalization();}}else        {// Localization Mode: Local Mapping is deactivated// Step 2：只進行跟蹤tracking，局部地圖不工作if(mState==LOST){// Step 2.1 如果跟丟了，只能重定位bOK = Relocalization();}else    {// mbVO是mbOnlyTracking為true時的才有的一個變量// mbVO為false表示此幀匹配了很多的MapPoints，跟蹤很正常 (注意有點反直覺)// mbVO為true表明此幀匹配了很少的MapPoints，少于10個，要跪的節奏if(!mbVO){// Step 2.2 如果跟蹤正常，使用恒速模型 或 參考關鍵幀跟蹤// In last frame we tracked enough MapPoints in the mapif(!mVelocity.empty()){bOK = TrackWithMotionModel();// ? 為了和前面模式統一，這個地方是不是應該加上// if(!bOK)//    bOK = TrackReferenceKeyFrame();}else{// 如果恒速模型不被滿足,那么就只能夠通過參考關鍵幀來定位bOK = TrackReferenceKeyFrame();}}else{// In last frame we tracked mainly "visual odometry" points.// We compute two camera poses, one from motion model and one doing relocalization.// If relocalization is sucessfull we choose that solution, otherwise we retain// the "visual odometry" solution.// mbVO為true，表明此幀匹配了很少（小于10）的地圖點，要跪的節奏，既做跟蹤又做重定位//MM=Motion Model,通過運動模型進行跟蹤的結果bool bOKMM = false;//通過重定位方法來跟蹤的結果bool bOKReloc = false;//運動模型中構造的地圖點vector<MapPoint*> vpMPsMM;//在追蹤運動模型后發現的外點vector<bool> vbOutMM;//運動模型得到的位姿cv::Mat TcwMM;// Step 2.3 當運動模型有效的時候,根據運動模型計算位姿if(!mVelocity.empty()){bOKMM = TrackWithMotionModel();// 將恒速模型跟蹤結果暫存到這幾個變量中，因為后面重定位會改變這些變量vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}// Step 2.4 使用重定位的方法來得到當前幀的位姿bOKReloc = Relocalization();// Step 2.5 根據前面的恒速模型、重定位結果來更新狀態if(bOKMM && !bOKReloc){// 恒速模型成功、重定位失敗，重新使用之前暫存的恒速模型結果mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;//? 疑似bug！這段代碼是不是重復增加了觀測次數？后面 TrackLocalMap 函數中會有這些操作// 如果當前幀匹配的3D點很少，增加當前可視地圖點的被觀測次數if(mbVO){// 更新當前幀的地圖點被觀測次數for(int i =0; i<mCurrentFrame.N; i++){//如果這個特征點形成了地圖點,并且也不是外點的時候if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){//增加能觀測到該地圖點的幀數mCurrentFrame.mvpMapPoints[i]->IncreaseFound();}}}}else if(bOKReloc){// 只要重定位成功整個跟蹤過程正常進行（重定位與跟蹤，更相信重定位）mbVO = false;}//有一個成功我們就認為執行成功了bOK = bOKReloc || bOKMM;}}}// 將最新的關鍵幀作為當前幀的參考關鍵幀mCurrentFrame.mpReferenceKF = mpReferenceKF;// If we have an initial estimation of the camera pose and matching. Track the local map.// Step 3：在跟蹤得到當前幀初始姿態后，現在對local map進行跟蹤得到更多的匹配，并優化當前位姿// 前面只是跟蹤一幀得到初始位姿，這里搜索局部關鍵幀、局部地圖點，和當前幀進行投影匹配，得到更多匹配的MapPoints后進行Pose優化if(!mbOnlyTracking){if(bOK)bOK = TrackLocalMap();}else{// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes// the camera we will use the local map again.// 重定位成功if(bOK && !mbVO)bOK = TrackLocalMap();}//根據上面的操作來判斷是否追蹤成功if(bOK)mState = OK;elsemState=LOST;// Step 4：更新顯示線程中的圖像、特征點、地圖點等信息mpFrameDrawer->Update(this);// If tracking were good, check if we insert a keyframe//只有在成功追蹤時才考慮生成關鍵幀的問題if(bOK){// Update motion model// Step 5：跟蹤成功，更新恒速運動模型if(!mLastFrame.mTcw.empty()){// 更新恒速運動模型 TrackWithMotionModel 中的mVelocitycv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));// mVelocity = Tcl = Tcw * Twl,表示上一幀到當前幀的變換， 其中 Twl = LastTwcmVelocity = mCurrentFrame.mTcw*LastTwc; }else//否則速度為空mVelocity = cv::Mat();//更新顯示中的位姿mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);// Clean VO matches// Step 6：清除觀測不到的地圖點   for(int i=0; i<mCurrentFrame.N; i++){MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(pMP)if(pMP->Observations()<1){mCurrentFrame.mvbOutlier[i] = false;mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Delete temporal MapPoints// Step 7：清除恒速模型跟蹤中 UpdateLastFrame中為當前幀臨時添加的MapPoints（僅雙目和rgbd）// 步驟6中只是在當前幀中將這些MapPoints剔除，這里從MapPoints數據庫中刪除// 臨時地圖點僅僅是為了提高雙目或rgbd攝像頭的幀間跟蹤效果，用完以后就扔了，沒有添加到地圖中for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;delete pMP;}// 這里不僅僅是清除mlpTemporalPoints，通過delete pMP還刪除了指針指向的MapPoint// 不能夠直接執行這個是因為其中存儲的都是指針,之前的操作都是為了避免內存泄露mlpTemporalPoints.clear();// Check if we need to insert a new keyframe// Step 8：檢測并插入關鍵幀，對于雙目或RGB-D會產生新的地圖點if(NeedNewKeyFrame())CreateNewKeyFrame();// We allow points with high innovation (considererd outliers by the Huber Function)// pass to the new keyframe, so that bundle adjustment will finally decide// if they are outliers or not. We don't want next frame to estimate its position// with those points so we discard them in the frame.// 作者這里說允許在BA中被Huber核函數判斷為外點的傳入新的關鍵幀中，讓后續的BA來審判他們是不是真正的外點// 但是估計下一幀位姿的時候我們不想用這些外點，所以刪掉//  Step 9 刪除那些在bundle adjustment中檢測為outlier的地圖點for(int i=0; i<mCurrentFrame.N;i++){// 這里第一個條件還要執行判斷是因為, 前面的操作中可能刪除了其中的地圖點if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Reset if the camera get lost soon after initialization// Step 10 如果初始化后不久就跟蹤失敗，并且relocation也沒有搞定，只能重新Resetif(mState==LOST){//如果地圖中的關鍵幀信息過少的話,直接重新進行初始化了if(mpMap->KeyFramesInMap()<=5){cout << "Track lost soon after initialisation, reseting..." << endl;mpSystem->Reset();return;}}//確保已經設置了參考關鍵幀if(!mCurrentFrame.mpReferenceKF)mCurrentFrame.mpReferenceKF = mpReferenceKF;// 保存上一幀的數據,當前幀變上一幀mLastFrame = Frame(mCurrentFrame);}// Store frame pose information to retrieve the complete camera trajectory afterwards.// Step 11：記錄位姿信息，用于最后保存所有的軌跡if(!mCurrentFrame.mTcw.empty()){// 計算相對姿態Tcr = Tcw * Twr, Twr = Trw^-1cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();//保存各種狀態mlRelativeFramePoses.push_back(Tcr);mlpReferences.push_back(mpReferenceKF);mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);mlbLost.push_back(mState==LOST);}else{// This can happen if tracking is lost// 如果跟蹤失敗，則相對位姿使用上一次值mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());mlpReferences.push_back(mlpReferences.back());mlFrameTimes.push_back(mlFrameTimes.back());mlbLost.push_back(mState==LOST);}}// Tracking 

7.11?Tracking流程中的關鍵問題(暗線)

?Tracking線程中初始化過程(Tracking::MonocularInitialization()和Tracking::StereoInitialization())會創建新的地圖點.
Tracking線程中創建新的關鍵幀(Tracking::CreateNewKeyFrame())會創建新的地圖點.
Tracking線程中根據恒速運動模型估計初始位姿(Tracking::TrackWithMotionModel())也會產生臨時地圖點,但這些臨時地圖點在跟蹤成功后會被馬上刪除.

所有的非臨時地圖點都是由關鍵幀建立的,Tracking::TrackWithMotionModel()中由非關鍵幀建立的關鍵點被設為臨時關鍵點,很快會被刪掉,僅作增強幀間匹配用,不會對建圖產生任何影響.這也不違反只有關鍵幀才能參與LocalMapping和LoppClosing線程的原則.

7.11.2 關鍵幀與地圖點間發生關系的時機

新創建出來的非臨時地圖點都會與創建它的關鍵幀建立雙向連接.
通過ORBmatcher::SearchByXXX()函數匹配得到的幀點關系只建立單向連接:
只在關鍵幀中添加了對地圖點的觀測(將地圖點加入到關鍵幀對象的成員變量mvpMapPoints中了).
沒有在地圖點中添加對關鍵幀的觀測(地圖點的成員變量mObservations中沒有該關鍵幀).
這為后文中LocalMapping線程中函數LocalMapping::ProcessNewKeyFrame()對關鍵幀中地圖點的處理埋下了伏筆.該函數通過檢查地圖點中是否有對關鍵點的觀測來判斷該地圖點是否是新生成的.

/*** @brief 處理列表中的關鍵幀，包括計算BoW、更新觀測、描述子、共視圖，插入到地圖等* */
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：計算該關鍵幀特征點的詞袋向量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); }}}}    // Update links in the Covisibility Graph// Step 4：更新關鍵幀間的連接關系（共視圖）mpCurrentKeyFrame->UpdateConnections();// Insert Keyframe in Map// Step 5：將該關鍵幀插入到地圖中mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

7.12 參考關鍵幀:?mpReferenceKF

參考關鍵幀的用途:
Tracking線程中函數Tracking::TrackReferenceKeyFrame()根據參考關鍵幀估計初始位姿.
用于初始化新創建的MapPoint的參考幀mpRefKF,函數MapPoint::UpdateNormalAndDepth()中根據參考關鍵幀mpRefKF更新地圖點的平均觀測距離.
參考關鍵幀的指定:
Traking線程中函數Tracking::CreateNewKeyFrame()創建完新關鍵幀后,會將新創建的關鍵幀設為參考關鍵幀.
Tracking線程中函數Tracking::TrackLocalMap()跟蹤局部地圖過程中調用函數Tracking::UpdateLocalMap(),其中調用函數Tracking::UpdateLocalKeyFrames(),將與當前幀共視程度最高的關鍵幀設為參考關鍵幀.

?8. ORB-SLAM2代碼詳解08_局部建圖線程LocalMapping

?8.1 各成員函數/變量

Tracking線程創建的所有關鍵幀都被插入到LocalMapping線程的緩沖隊列mlNewKeyFrames中.

成員函數mbAcceptKeyFrames表示當前LocalMapping線程是否愿意接收關鍵幀,這會被Tracking線程函數Tracking::NeedNewKeyFrame()用作是否生產關鍵幀的參考因素之一;但即使mbAcceptKeyFrames為false,在系統很需要關鍵幀的情況下Tracking線程函數Tracking::NeedNewKeyFrame()也會決定生成關鍵幀.

8.2 局部建圖主函數:?Run()

函數LocalMapping::Run()是LocalMapping線程的主函數,該函數內部是一個死循環,每3毫秒查詢一次當前線程緩沖隊列mlNewKeyFrames.若查詢到了待處理的新關鍵幀,就進行查詢?

// 線程主函數
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;// 已經處理完隊列中的最后的一個關鍵幀，并且閉環檢測沒有請求停止LocalMappingif(!CheckNewKeyFrames() && !stopRequested()){// Local BA// Step 6 當局部地圖中的關鍵幀大于2個的時候進行局部地圖的BAif(mpMap->KeyFramesInMap()>2)// 注意這里的第二個參數是按地址傳遞的,當這里的 mbAbortBA 狀態發生變化時，能夠及時執行/停止BAOptimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);// Check redundant local Keyframes// Step 7 檢測并剔除當前幀相鄰的關鍵幀中冗余的關鍵幀// 冗余的判定：該關鍵幀的90%的地圖點可以被其它關鍵幀觀測到KeyFrameCulling();}// Step 8 將當前幀加入到閉環檢測隊列中// 注意這里的關鍵幀被設置成為了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();
}

8.3 處理隊列中第一個關鍵幀:?ProcessNewKeyFrame()

?

在第3步中處理當前關鍵點時比較有意思,通過判斷該地圖點是否觀測到當前關鍵幀(pMP->IsInKeyFrame(mpCurrentKeyFrame))來判斷該地圖點是否是當前關鍵幀中新生成的.

若地圖點是本關鍵幀跟蹤過程中匹配得到的(Tracking::TrackWithMotionModel()、Tracking::TrackReferenceKeyFrame()、Tracking::Relocalization()和Tracking::SearchLocalPoints()中調用了ORBmatcher::SearchByProjection()和ORBmatcher::SearchByBoW()方法),則是之前關鍵幀中創建的地圖點,只需添加其對當前幀的觀測即可.
若地圖點是本關鍵幀跟蹤過程中新生成的(包括:1.單目或雙目初始化Tracking::MonocularInitialization()、Tracking::StereoInitialization();2.創建新關鍵幀Tracking::CreateNewKeyFrame()),則該地圖點中有對當前關鍵幀的觀測,是新生成的地圖點,放入容器mlNewKeyFrames中供LocalMapping::MapPointCulling()函數篩選.
?

/*** @brief 處理列表中的關鍵幀，包括計算BoW、更新觀測、描述子、共視圖，插入到地圖等* */
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：計算該關鍵幀特征點的詞袋向量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); }}}}    // Update links in the Covisibility Graph// Step 4：更新關鍵幀間的連接關系（共視圖）mpCurrentKeyFrame->UpdateConnections();// Insert Keyframe in Map// Step 5：將該關鍵幀插入到地圖中mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

8.4 剔除壞地圖點:?MapPointCulling()

?冗余地圖點的標準:滿足以下其中之一就算是壞地圖點

1?

2 在創建的3幀內觀測數目少于2(雙目為3)

若地圖點經過了連續3個關鍵幀仍未被剔除,則被認為是好的地圖點

/*** @brief 檢查新增地圖點，根據地圖點的觀測情況剔除質量不好的新增的地圖點* mlpRecentAddedMapPoints：存儲新增的地圖點，這里是要刪除其中不靠譜的*/
void LocalMapping::MapPointCulling()
{// Check Recent Added MapPointslist<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;// Step 1：根據相機類型設置不同的觀測閾值int nThObs;if(mbMonocular)nThObs = 2;elsenThObs = 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);elselit++;}
}

?MapPoint類中關于召回率的成員函數和變量如下:

這兩個成員變量主要用于Tracking線程.

在函數Tracking::SearchLocalPoints()中,會對所有處于當前幀視錐內的地圖點調用成員函數MapPoint::IncreaseVisible().(這些點未必真的被當前幀觀測到了,只是地理位置上處于當前幀視錐范圍內).
?

void Tracking::SearchLocalPoints() {// 當前關鍵幀的地圖點for (MapPoint *pMP : mCurrentFrame.mvpMapPoints) {pMP->IncreaseVisible();}}}
?// 局部關鍵幀中不屬于當前幀,但在當前幀視錐范圍內的地圖點for (MapPoint *pMP = *vit : mvpLocalMapPoints.begin()) {if (mCurrentFrame.isInFrustum(pMP, 0.5)) {pMP->IncreaseVisible();}}
?// ...
}

在函數Tracking::TrackLocalMap()中,會對所有當前幀觀測到的地圖點調用MaoPoint::IncreaseFound().

bool Tracking::TrackLocalMap() {// ...for (int i = 0; i < mCurrentFrame.N; i++) {if (mCurrentFrame.mvpMapPoints[i]) {if (!mCurrentFrame.mvbOutlier[i]) {// 當前幀觀測到的地圖點mCurrentFrame.mvpMapPoints[i]->IncreaseFound();// ...}}}// ...
}

8.5 創建新地圖點:?CreateNewMapPoints()

將當前關鍵幀分別與共視程度最高的前10(單目相機取20)個共視關鍵幀兩兩進行特征匹配,生成地圖點.

對于雙目相機的匹配特征點對,可以根據某幀特征點深度恢復地圖點,也可以根據兩幀間對極幾何三角化地圖點,這里取視差角最大的方式來生成地圖點.

?8.6 融合當前關鍵幀和其共視幀的地圖點:?SearchInNeighbors()

?

本函數將當前關鍵幀與其一級和二級共視關鍵幀做地圖點融合,分兩步:

1. 正向融合: 將當前關鍵幀的地圖點融合到各共視關鍵幀中.
2. 反向融合: 將各共視關鍵幀的地圖點融合到當前關鍵幀中.

/*** @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：遍歷每一個一級鄰接和二級鄰接關鍵幀，收集他們的地圖點存儲到 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);// 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()將地圖點與幀中圖像的特征點匹配,實現地圖點融合.

在將地圖點反投影到幀中的過程中,存在以下兩種情況:

1. 若地圖點反投影對應位置上不存在地圖點,則直接添加觀測.
2. 若地圖點反投影位置上存在對應地圖點,則將兩個地圖點合并到其中觀測較多的那個.

/*** @brief 將地圖點投影到關鍵幀中進行匹配和融合；融合策略如下* 1.如果地圖點能匹配關鍵幀的特征點，并且該點有對應的地圖點，那么選擇觀測數目多的替換兩個地圖點* 2.如果地圖點能匹配關鍵幀的特征點，并且該點沒有對應的地圖點，那么為該點添加該投影地圖點* @param[in] pKF           關鍵幀* @param[in] vpMapPoints   待投影的地圖點* @param[in] th            搜索窗口的閾值，默認為3* @return int              更新地圖點的數量*/
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++){MapPoint* pMP = vpMapPoints[i];// Step 1 判斷地圖點的有效性 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;// 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// 確定搜索范圍const float radius = th*pKF->mvScaleFactors[nPredictedLevel];// Step 5 在投影點附近搜索窗口內找到候選匹配點的索引const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);if(vIndices.empty())continue;// 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.82if(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){bestDist = dist;bestIdx = idx;}}// 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);elsepMPinKF->Replace(pMP);}}else{// 如果最佳匹配點沒有對應地圖點，添加觀測信息pMP->AddObservation(pKF,bestIdx);pKF->AddMapPoint(pMP,bestIdx);}nFused++;}}return nFused;
}

?8.7 局部BA優化:?Optimizer::LocalBundleAdjustment()

局部BA優化當前幀的局部地圖.

• 當前關鍵幀的一級共視關鍵幀位姿會被優化;二極共視關鍵幀會加入優化圖,但其位姿不會被優化.
• 所有局部地圖點位姿都會被優化.

8.8 剔除冗余關鍵幀:?KeyFrameCulling()

/*** @brief 檢測當前關鍵幀在共視圖中的關鍵幀，根據地圖點在共視圖中的冗余程度剔除該共視關鍵幀* 冗余關鍵幀的判定：90%以上的地圖點能被其他關鍵幀（至少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// 該函數里變量層層深入，這里列一下：// 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;// 第1個關鍵幀不能刪除，跳過if(pKF->mnId==0)continue;// Step 2：提取每個共視關鍵幀的地圖點const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();// 記錄某個點被觀測次數，后面并未使用int nObs = 3;                     // 觀測次數閾值，默認為3const 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存儲的是可以看到該地圖點的所有關鍵幀的集合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();}
}

?9. ORB-SLAM2代碼詳解09_閉環線程LoopClosing

?

9.1 各成員函數/變量

// 線程主函數
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;// 已經處理完隊列中的最后的一個關鍵幀，并且閉環檢測沒有請求停止LocalMappingif(!CheckNewKeyFrames() && !stopRequested()){// Local BA// Step 6 當局部地圖中的關鍵幀大于2個的時候進行局部地圖的BAif(mpMap->KeyFramesInMap()>2)// 注意這里的第二個參數是按地址傳遞的,當這里的 mbAbortBA 狀態發生變化時，能夠及時執行/停止BAOptimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);// Check redundant local Keyframes// Step 7 檢測并剔除當前幀相鄰的關鍵幀中冗余的關鍵幀// 冗余的判定：該關鍵幀的90%的地圖點可以被其它關鍵幀觀測到KeyFrameCulling();}// Step 8 將當前幀加入到閉環檢測隊列中// 注意這里的關鍵幀被設置成為了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();
}

?9.2 閉環檢測:?DetectLoop()

?LoopClosing類中定義類型ConsistentGroup,表示關鍵幀組.

typedef pair<set<KeyFrame *>, int> ConsistentGroup

• 第一個元素表示一組共視關鍵幀.
• 第二個元素表示該關鍵幀組的連續長度.

所謂連續,指的是兩個關鍵幀組中存在相同的關鍵幀.

?閉環檢測原理: 若連續4個關鍵幀都能在數據庫中找到對應的閉環匹配關鍵幀組,且這些閉環匹配關鍵幀組間是連續的,則認為實現閉環,

具體來說,回環檢測過程如下:

1. 找到當前關鍵幀的閉環候選關鍵幀vpCandidateKFs:

   閉環候選關鍵幀取自于與當前關鍵幀具有相同的BOW向量但不存在直接連接的關鍵幀.

?

2 將閉環候選關鍵幀和其共視關鍵幀組合成為關鍵幀組vCurrentConsistentGroups:?

3 在當前關鍵組和之前的連續關鍵組間尋找連續關系.

若當前關鍵幀組在之前的連續關鍵幀組中找到連續關系,則當前的連續關鍵幀組的連續長度加1.
若當前關鍵幀組在之前的連續關鍵幀組中沒能找到連續關系,則當前關鍵幀組的連續長度為0.
關鍵幀組的連續關系是指兩個關鍵幀組間是否有關鍵幀同時存在于兩關鍵幀組中.

?若某關鍵幀組的連續長度達到3,則認為該關鍵幀實現閉環.

/*** @brief 閉環檢測* * @return true             成功檢測到閉環* @return false            未檢測到閉環*/
bool LoopClosing::DetectLoop()
{{// Step 1 從隊列中取出一個關鍵幀,作為當前檢測閉環關鍵幀unique_lock<mutex> lock(mMutexLoopQueue);// 從隊列頭開始取，也就是先取早進來的關鍵幀mpCurrentKF = mlpLoopKeyFrameQueue.front();// 取出關鍵幀后從隊列里彈出該關鍵幀mlpLoopKeyFrameQueue.pop_front();// Avoid that a keyframe can be erased while it is being process by this thread// 設置當前關鍵幀不要在優化的過程中被刪除mpCurrentKF->SetNotErase();}//If the map contains less than 10 KF or less than 10 KF have passed from last loop detection// Step 2：如果距離上次閉環沒多久（小于10幀），或者map中關鍵幀總共還沒有10幀，則不進行閉環檢測// 后者的體現是當mLastLoopKFid為0的時候if(mpCurrentKF->mnId<mLastLoopKFid+10){mpKeyFrameDB->add(mpCurrentKF);mpCurrentKF->SetErase();return false;}// Compute reference BoW similarity score// This is the lowest score to a connected keyframe in the covisibility graph// We will impose loop candidates to have a higher similarity than this// Step 3：遍歷當前回環關鍵幀所有連接（>15個共視地圖點）關鍵幀，計算當前關鍵幀與每個共視關鍵的bow相似度得分，并得到最低得分minScoreconst vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;float minScore = 1;for(size_t i=0; i<vpConnectedKeyFrames.size(); i++){KeyFrame* pKF = vpConnectedKeyFrames[i];if(pKF->isBad())continue;const DBoW2::BowVector &BowVec = pKF->mBowVec;// 計算兩個關鍵幀的相似度得分；得分越低,相似度越低float score = mpORBVocabulary->score(CurrentBowVec, BowVec);// 更新最低得分if(score<minScore)minScore = score;}// Query the database imposing the minimum score// Step 4：在所有關鍵幀中找出閉環候選幀（注意不和當前幀連接）// minScore的作用：認為和當前關鍵幀具有回環關系的關鍵幀,不應該低于當前關鍵幀的相鄰關鍵幀的最低的相似度minScore// 得到的這些關鍵幀,和當前關鍵幀具有較多的公共單詞,并且相似度評分都挺高vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);// If there are no loop candidates, just add new keyframe and return false// 如果沒有閉環候選幀，返回falseif(vpCandidateKFs.empty()){mpKeyFrameDB->add(mpCurrentKF);mvConsistentGroups.clear();mpCurrentKF->SetErase();return false;           }// For each loop candidate check consistency with previous loop candidates// Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)// A group is consistent with a previous group if they share at least a keyframe// We must detect a consistent loop in several consecutive keyframes to accept it// Step 5：在候選幀中檢測具有連續性的候選幀// 1、每個候選幀將與自己相連的關鍵幀構成一個“子候選組spCandidateGroup”， vpCandidateKFs-->spCandidateGroup// 2、檢測“子候選組”中每一個關鍵幀是否存在于“連續組”，如果存在 nCurrentConsistency++，則將該“子候選組”放入“當前連續組vCurrentConsistentGroups”// 3、如果nCurrentConsistency大于等于3，那么該”子候選組“代表的候選幀過關，進入mvpEnoughConsistentCandidates// 相關的概念說明:（為方便理解，見視頻里的圖示）// 組(group): 對于某個關鍵幀, 其和其具有共視關系的關鍵幀組成了一個"組";// 子候選組(CandidateGroup): 對于某個候選的回環關鍵幀, 其和其具有共視關系的關鍵幀組成的一個"組";// 連續(Consistent):  不同的組之間如果共同擁有一個及以上的關鍵幀,那么稱這兩個組之間具有連續關系// 連續性(Consistency):稱之為連續長度可能更合適,表示累計的連續的鏈的長度:A--B 為1, A--B--C--D 為3等;具體反映在數據類型 ConsistentGroup.second上// 連續組(Consistent group): mvConsistentGroups存儲了上次執行回環檢測時, 新的被檢測出來的具有連續性的多個組的集合.由于組之間的連續關系是個網狀結構,因此可能存在//                          一個組因為和不同的連續組鏈都具有連續關系,而被添加兩次的情況(當然連續性度量是不相同的)// 連續組鏈:自造的稱呼,類似于菊花鏈A--B--C--D這樣形成了一條連續組鏈.對于這個例子中,由于可能E,F都和D有連續關系,因此連續組鏈會產生分叉;為了簡化計算,連續組中將只會保存//         最后形成連續關系的連續組們(見下面的連續組的更新)// 子連續組: 上面的連續組中的一個組// 連續組的初始值: 在遍歷某個候選幀的過程中,如果該子候選組沒有能夠和任何一個上次的子連續組產生連續關系,那么就將添加自己組為連續組,并且連續性為0(相當于新開了一個連續鏈)// 連續組的更新: 當前次回環檢測過程中,所有被檢測到和之前的連續組鏈有連續的關系的組,都將在對應的連續組鏈后面+1,這些子候選組(可能有重復,見上)都將會成為新的連續組;//              換而言之連續組mvConsistentGroups中只保存連續組鏈中末尾的組// 最終篩選后得到的閉環幀mvpEnoughConsistentCandidates.clear();// ConsistentGroup數據類型為pair<set<KeyFrame*>,int>// ConsistentGroup.first對應每個“連續組”中的關鍵幀，ConsistentGroup.second為每個“連續組”的已連續幾個的序號vector<ConsistentGroup> vCurrentConsistentGroups;// 這個下標是每個"子連續組"的下標,bool表示當前的候選組中是否有和該組相同的一個關鍵幀vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);// Step 5.1：遍歷剛才得到的每一個候選關鍵幀for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++){KeyFrame* pCandidateKF = vpCandidateKFs[i];// Step 5.2：將自己以及與自己相連的關鍵幀構成一個“子候選組”set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();// 把自己也加進去spCandidateGroup.insert(pCandidateKF);// 連續性達標的標志bool bEnoughConsistent = false;bool bConsistentForSomeGroup = false;// Step 5.3：遍歷前一次閉環檢測到的連續組鏈// 上一次閉環的連續組鏈 std::vector<ConsistentGroup> mvConsistentGroups// 其中ConsistentGroup的定義：typedef pair<set<KeyFrame*>,int> ConsistentGroup// 其中 ConsistentGroup.first對應每個“連續組”中的關鍵幀集合，ConsistentGroup.second為每個“連續組”的連續長度for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++){// 取出之前的一個子連續組中的關鍵幀集合set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;// Step 5.4：遍歷每個“子候選組”，檢測子候選組中每一個關鍵幀在“子連續組”中是否存在// 如果有一幀共同存在于“子候選組”與之前的“子連續組”，那么“子候選組”與該“子連續組”連續bool bConsistent = false;for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++){if(sPreviousGroup.count(*sit)){// 如果存在，該“子候選組”與該“子連續組”相連bConsistent=true;// 該“子候選組”至少與一個”子連續組“相連，跳出循環bConsistentForSomeGroup=true;break;}}if(bConsistent){// Step 5.5：如果判定為連續，接下來判斷是否達到連續的條件// 取出和當前的候選組發生"連續"關系的子連續組的"已連續次數"int nPreviousConsistency = mvConsistentGroups[iG].second;// 將當前候選組連續長度在原子連續組的基礎上 +1，int nCurrentConsistency = nPreviousConsistency + 1;// 如果上述連續關系還未記錄到 vCurrentConsistentGroups，那么記錄一下// 注意這里spCandidateGroup 可能放置在vbConsistentGroup中其他索引(iG)下if(!vbConsistentGroup[iG]){// 將該“子候選組”的該關鍵幀打上連續編號加入到“當前連續組”ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);// 放入本次閉環檢測的連續組vCurrentConsistentGroups里vCurrentConsistentGroups.push_back(cg);//this avoid to include the same group more than once// 標記一下，防止重復添加到同一個索引iG// 但是spCandidateGroup可能重復添加到不同的索引iG對應的vbConsistentGroup 中vbConsistentGroup[iG]=true; }// 如果連續長度滿足要求，那么當前的這個候選關鍵幀是足夠靠譜的// 連續性閾值 mnCovisibilityConsistencyTh=3// 足夠連續的標記 bEnoughConsistentif(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent){// 記錄為達到連續條件了mvpEnoughConsistentCandidates.push_back(pCandidateKF);//this avoid to insert the same candidate more than once// 標記一下，防止重復添加bEnoughConsistent=true; // ? 這里可以break掉結束當前for循環嗎？// 回答：不行。因為雖然pCandidateKF達到了連續性要求// 但spCandidateGroup 還可以和mvConsistentGroups 中其他的子連續組進行連接}}}// If the group is not consistent with any previous group insert with consistency counter set to zero// Step 5.6：如果該“子候選組”的所有關鍵幀都和上次閉環無關（不連續），vCurrentConsistentGroups 沒有新添加連續關系// 于是就把“子候選組”全部拷貝到 vCurrentConsistentGroups， 用于更新mvConsistentGroups，連續性計數器設為0if(!bConsistentForSomeGroup){ConsistentGroup cg = make_pair(spCandidateGroup,0);vCurrentConsistentGroups.push_back(cg);}}// 遍歷得到的初級的候選關鍵幀// Update Covisibility Consistent Groups// 更新連續組mvConsistentGroups = vCurrentConsistentGroups;// Add Current Keyframe to database// 當前閉環檢測的關鍵幀添加到關鍵幀數據庫中mpKeyFrameDB->add(mpCurrentKF);if(mvpEnoughConsistentCandidates.empty()){// 未檢測到閉環，返回falsempCurrentKF->SetErase();return false;}else {// 成功檢測到閉環，返回truereturn true;}// 多余的代碼,執行不到mpCurrentKF->SetErase();return false;
}

當前關鍵幀的閉環候選關鍵幀取自于與當前關鍵幀具有相同BOW向量但不直接相連的關鍵幀.

/*** @brief 在閉環檢測中找到與該關鍵幀可能閉環的關鍵幀（注意不和當前幀連接）* Step 1：找出和當前幀具有公共單詞的所有關鍵幀，不包括與當前幀連接的關鍵幀* Step 2：只和具有共同單詞較多的（最大數目的80%以上）關鍵幀進行相似度計算 * Step 3：計算上述候選幀對應的共視關鍵幀組的總得分，只取最高組得分75%以上的組* Step 4：得到上述組中分數最高的關鍵幀作為閉環候選關鍵幀* @param[in] pKF               需要閉環檢測的關鍵幀* @param[in] minScore          候選閉環關鍵幀幀和當前關鍵幀的BoW相似度至少要大于minScore* @return vector<KeyFrame*>    閉環候選關鍵幀*/
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{// 取出與當前關鍵幀相連（>15個共視地圖點）的所有關鍵幀，這些相連關鍵幀都是局部相連，在閉環檢測的時候將被剔除// 相連關鍵幀定義見 KeyFrame::UpdateConnections()set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();// 用于保存可能與當前關鍵幀形成閉環的候選幀（只要有相同的word，且不屬于局部相連（共視）幀）list<KeyFrame*> lKFsSharingWords;// Search all keyframes that share a word with current keyframes// Discard keyframes connected to the query keyframe// Step 1：找出和當前幀具有公共單詞的所有關鍵幀，不包括與當前幀連接的關鍵幀{unique_lock<mutex> lock(mMutex);// words是檢測圖像是否匹配的樞紐，遍歷該pKF的每一個word// mBowVec 內部實際存儲的是std::map<WordId, WordValue>// WordId 和 WordValue 表示Word在葉子中的id 和權重for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++){// 提取所有包含該word的KeyFramelist<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];// 然后對這些關鍵幀展開遍歷for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++){KeyFrame* pKFi=*lit;if(pKFi->mnLoopQuery!=pKF->mnId)    {// 還沒有標記為pKF的閉環候選幀pKFi->mnLoopWords=0;// 和當前關鍵幀共視的話不作為閉環候選幀if(!spConnectedKeyFrames.count(pKFi)){// 沒有共視就標記作為閉環候選關鍵幀，放到lKFsSharingWords里pKFi->mnLoopQuery=pKF->mnId;lKFsSharingWords.push_back(pKFi);}}pKFi->mnLoopWords++;// 記錄pKFi與pKF具有相同word的個數}}}// 如果沒有關鍵幀和這個關鍵幀具有相同的單詞,那么就返回空if(lKFsSharingWords.empty())return vector<KeyFrame*>();list<pair<float,KeyFrame*> > lScoreAndMatch;// Only compare against those keyframes that share enough words// Step 2：統計上述所有閉環候選幀中與當前幀具有共同單詞最多的單詞數，用來決定相對閾值 int maxCommonWords=0;for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++){if((*lit)->mnLoopWords>maxCommonWords)maxCommonWords=(*lit)->mnLoopWords;}// 確定最小公共單詞數為最大公共單詞數目的0.8倍int minCommonWords = maxCommonWords*0.8f;int nscores=0;// Compute similarity score. Retain the matches whose score is higher than minScore// Step 3：遍歷上述所有閉環候選幀，挑選出共有單詞數大于minCommonWords且單詞匹配度大于minScore存入lScoreAndMatchfor(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++){KeyFrame* pKFi = *lit;// pKF只和具有共同單詞較多（大于minCommonWords）的關鍵幀進行比較if(pKFi->mnLoopWords>minCommonWords){nscores++;// 這個變量后面沒有用到// 用mBowVec來計算兩者的相似度得分float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);pKFi->mLoopScore = si;if(si>=minScore)lScoreAndMatch.push_back(make_pair(si,pKFi));}}// 如果沒有超過指定相似度閾值的，那么也就直接跳過去if(lScoreAndMatch.empty())return vector<KeyFrame*>();list<pair<float,KeyFrame*> > lAccScoreAndMatch;float bestAccScore = minScore;// Lets now accumulate score by covisibility// 單單計算當前幀和某一關鍵幀的相似性是不夠的，這里將與關鍵幀相連（權值最高，共視程度最高）的前十個關鍵幀歸為一組，計算累計得分// Step 4：計算上述候選幀對應的共視關鍵幀組的總得分，得到最高組得分bestAccScore，并以此決定閾值minScoreToRetainfor(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++){KeyFrame* pKFi = it->second;vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);float bestScore = it->first; // 該組最高分數float accScore = it->first;  // 該組累計得分KeyFrame* pBestKF = pKFi;    // 該組最高分數對應的關鍵幀// 遍歷共視關鍵幀，累計得分 for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++){KeyFrame* pKF2 = *vit;// 只有pKF2也在閉環候選幀中，且公共單詞數超過最小要求，才能貢獻分數if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords){accScore+=pKF2->mLoopScore;// 統計得到組里分數最高的關鍵幀if(pKF2->mLoopScore>bestScore){pBestKF=pKF2;bestScore = pKF2->mLoopScore;}}}lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));// 記錄所有組中組得分最高的組，用于確定相對閾值if(accScore>bestAccScore)bestAccScore=accScore;}// Return all those keyframes with a score higher than 0.75*bestScore// 所有組中最高得分的0.75倍，作為最低閾值float minScoreToRetain = 0.75f*bestAccScore;set<KeyFrame*> spAlreadyAddedKF;vector<KeyFrame*> vpLoopCandidates;vpLoopCandidates.reserve(lAccScoreAndMatch.size());// Step 5：只取組得分大于閾值的組，得到組中分數最高的關鍵幀們作為閉環候選關鍵幀for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++){if(it->first>minScoreToRetain){KeyFrame* pKFi = it->second;// spAlreadyAddedKF 是為了防止重復添加if(!spAlreadyAddedKF.count(pKFi)){vpLoopCandidates.push_back(pKFi);spAlreadyAddedKF.insert(pKFi);}}}return vpLoopCandidates;
}

9.3 計算Sim3變換:?ComputeSim3()

/*** @brief 計算當前關鍵幀和上一步閉環候選幀的Sim3變換* 1. 遍歷閉環候選幀集，篩選出與當前幀的匹配特征點數大于20的候選幀集合，并為每一個候選幀構造一個Sim3Solver* 2. 對每一個候選幀進行 Sim3Solver 迭代匹配，直到有一個候選幀匹配成功，或者全部失敗* 3. 取出閉環匹配上關鍵幀的相連關鍵幀，得到它們的地圖點放入 mvpLoopMapPoints* 4. 將閉環匹配上關鍵幀以及相連關鍵幀的地圖點投影到當前關鍵幀進行投影匹配* 5. 判斷當前幀與檢測出的所有閉環關鍵幀是否有足夠多的地圖點匹配* 6. 清空mvpEnoughConsistentCandidates* @return true         只要有一個候選關鍵幀通過Sim3的求解與優化，就返回true* @return false        所有候選關鍵幀與當前關鍵幀都沒有有效Sim3變換*/
bool LoopClosing::ComputeSim3()
{// Sim3 計算流程說明：// 1. 通過Bow加速描述子的匹配，利用RANSAC粗略地計算出當前幀與閉環幀的Sim3（當前幀---閉環幀）          // 2. 根據估計的Sim3，對3D點進行投影找到更多匹配，通過優化的方法計算更精確的Sim3（當前幀---閉環幀）   // 3. 將閉環幀以及閉環幀相連的關鍵幀的地圖點與當前幀的點進行匹配（當前幀---閉環幀+相連關鍵幀）     // 注意以上匹配的結果均都存在成員變量mvpCurrentMatchedPoints中，實際的更新步驟見CorrectLoop()步驟3// 對于雙目或者是RGBD輸入的情況,計算得到的尺度=1//  準備工作// For each consistent loop candidate we try to compute a Sim3// 對每個（上一步得到的具有足夠連續關系的）閉環候選幀都準備算一個Sim3const int nInitialCandidates = mvpEnoughConsistentCandidates.size();// We compute first ORB matches for each candidate// If enough matches are found, we setup a Sim3SolverORBmatcher matcher(0.75,true);// 存儲每一個候選幀的Sim3Solver求解器vector<Sim3Solver*> vpSim3Solvers;vpSim3Solvers.resize(nInitialCandidates);// 存儲每個候選幀的匹配地圖點信息vector<vector<MapPoint*> > vvpMapPointMatches;vvpMapPointMatches.resize(nInitialCandidates);// 存儲每個候選幀應該被放棄(True）或者 保留(False)vector<bool> vbDiscarded;vbDiscarded.resize(nInitialCandidates);// 完成 Step 1 的匹配后，被保留的候選幀數量int nCandidates=0;// Step 1. 遍歷閉環候選幀集，初步篩選出與當前關鍵幀的匹配特征點數大于20的候選幀集合，并為每一個候選幀構造一個Sim3Solverfor(int i=0; i<nInitialCandidates; i++){// Step 1.1 從篩選的閉環候選幀中取出一幀有效關鍵幀pKFKeyFrame* pKF = mvpEnoughConsistentCandidates[i];// 避免在LocalMapping中KeyFrameCulling函數將此關鍵幀作為冗余幀剔除pKF->SetNotErase();// 如果候選幀質量不高，直接PASSif(pKF->isBad()){vbDiscarded[i] = true;continue;}// Step 1.2 將當前幀 mpCurrentKF 與閉環候選關鍵幀pKF匹配// 通過bow加速得到 mpCurrentKF 與 pKF 之間的匹配特征點// vvpMapPointMatches 是匹配特征點對應的地圖點,本質上來自于候選閉環幀int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);// 粗篩：匹配的特征點數太少，該候選幀剔除if(nmatches<20){vbDiscarded[i] = true;continue;}else{// Step 1.3 為保留的候選幀構造Sim3求解器// 如果 mbFixScale（是否固定尺度） 為 true，則是6 自由度優化（雙目 RGBD）// 如果是false，則是7 自由度優化（單目）Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);// Sim3Solver Ransac 過程置信度0.99，至少20個inliers 最多300次迭代pSolver->SetRansacParameters(0.99,20,300);vpSim3Solvers[i] = pSolver;}// 保留的候選幀數量nCandidates++;}// 用于標記是否有一個候選幀通過Sim3Solver的求解與優化bool bMatch = false;// Step 2 對每一個候選幀用Sim3Solver 迭代匹配，直到有一個候選幀匹配成功，或者全部失敗while(nCandidates>0 && !bMatch){// 遍歷每一個候選幀for(int i=0; i<nInitialCandidates; i++){if(vbDiscarded[i])continue;KeyFrame* pKF = mvpEnoughConsistentCandidates[i];// 內點（Inliers）標志// 即標記經過RANSAC sim3 求解后,vvpMapPointMatches中的哪些作為內點vector<bool> vbInliers; // 內點（Inliers）數量int nInliers;// 是否到達了最優解bool bNoMore;// Step 2.1 取出從 Step 1.3 中為當前候選幀構建的 Sim3Solver 并開始迭代Sim3Solver* pSolver = vpSim3Solvers[i];// 最多迭代5次，返回的Scm是候選幀pKF到當前幀mpCurrentKF的Sim3變換（T12）cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);// If Ransac reachs max. iterations discard keyframe// 總迭代次數達到最大限制還沒有求出合格的Sim3變換，該候選幀剔除if(bNoMore){vbDiscarded[i]=true;nCandidates--;}// If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences// 如果計算出了Sim3變換，繼續匹配出更多點并優化。因為之前 SearchByBoW 匹配可能會有遺漏if(!Scm.empty()){// 取出經過Sim3Solver 后匹配點中的內點集合vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));for(size_t j=0, jend=vbInliers.size(); j<jend; j++){// 保存內點if(vbInliers[j])vpMapPointMatches[j]=vvpMapPointMatches[i][j];}// Step 2.2 通過上面求取的Sim3變換引導關鍵幀匹配，彌補Step 1中的漏匹配// 候選幀pKF到當前幀mpCurrentKF的R（R12），t（t12），變換尺度s（s12）cv::Mat R = pSolver->GetEstimatedRotation();cv::Mat t = pSolver->GetEstimatedTranslation();const float s = pSolver->GetEstimatedScale();// 查找更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特征點數，之前使用SearchByBoW進行特征點匹配時會有漏匹配）// 通過Sim3變換，投影搜索pKF1的特征點在pKF2中的匹配，同理，投影搜索pKF2的特征點在pKF1中的匹配// 只有互相都成功匹配的才認為是可靠的匹配matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);// Step 2.3 用新的匹配來優化 Sim3，只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷// OpenCV的Mat矩陣轉成Eigen的Matrix類型// gScm：候選關鍵幀到當前幀的Sim3變換g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);// 如果mbFixScale為true，則是6 自由度優化（雙目 RGBD），如果是false，則是7 自由度優化（單目）// 優化mpCurrentKF與pKF對應的MapPoints間的Sim3，得到優化后的量gScmconst int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);// 如果優化成功，則停止while循環遍歷閉環候選if(nInliers>=20){// 為True時將不再進入 while循環bMatch = true;// mpMatchedKF就是最終閉環檢測出來與當前幀形成閉環的關鍵幀mpMatchedKF = pKF;// gSmw：從世界坐標系 w 到該候選幀 m 的Sim3變換，都在一個坐標系下，所以尺度 Scale=1g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);// 得到g2o優化后從世界坐標系到當前幀的Sim3變換mg2oScw = gScm*gSmw;mScw = Converter::toCvMat(mg2oScw);mvpCurrentMatchedPoints = vpMapPointMatches;// 只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷break;}}}}// 退出上面while循環的原因有兩種,一種是求解到了bMatch置位后出的,另外一種是nCandidates耗盡為0if(!bMatch){// 如果沒有一個閉環匹配候選幀通過Sim3的求解與優化// 清空mvpEnoughConsistentCandidates，這些候選關鍵幀以后都不會在再參加回環檢測過程了for(int i=0; i<nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();// 當前關鍵幀也將不會再參加回環檢測了mpCurrentKF->SetErase();// Sim3 計算失敗，退出了return false;}// Step 3：取出與當前幀閉環匹配上的關鍵幀及其共視關鍵幀，以及這些共視關鍵幀的地圖點// 注意是閉環檢測出來與當前幀形成閉環的關鍵幀 mpMatchedKF// 將mpMatchedKF共視的關鍵幀全部取出來放入 vpLoopConnectedKFs// 將vpLoopConnectedKFs的地圖點取出來放入mvpLoopMapPointsvector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();// 包含閉環匹配關鍵幀本身,形成一個“閉環關鍵幀小組“vpLoopConnectedKFs.push_back(mpMatchedKF);mvpLoopMapPoints.clear();// 遍歷這個組中的每一個關鍵幀for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++){KeyFrame* pKF = *vit;vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();// 遍歷其中一個關鍵幀的所有有效地圖點for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++){MapPoint* pMP = vpMapPoints[i];if(pMP){// mnLoopPointForKF 用于標記，避免重復添加if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId){mvpLoopMapPoints.push_back(pMP);// 標記一下pMP->mnLoopPointForKF=mpCurrentKF->mnId;}}}}// Find more matches projecting with the computed Sim3// Step 4：將閉環關鍵幀及其連接關鍵幀的所有地圖點投影到當前關鍵幀進行投影匹配// 根據投影查找更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特征點數）// 根據Sim3變換，將每個mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配對// mvpCurrentMatchedPoints是前面經過SearchBySim3得到的已經匹配的點對，這里就忽略不再匹配了// 搜索范圍系數為10matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);// If enough matches accept Loop// Step 5: 統計當前幀與閉環關鍵幀的匹配地圖點數目，超過40個說明成功閉環，否則失敗int nTotalMatches = 0;for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++){if(mvpCurrentMatchedPoints[i])nTotalMatches++;}if(nTotalMatches>=40){// 如果當前回環可靠,保留當前待閉環關鍵幀，其他閉環候選全部刪掉以后不用了for(int i=0; i<nInitialCandidates; i++)if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)mvpEnoughConsistentCandidates[i]->SetErase();return true;}else   {// 閉環不可靠，閉環候選及當前待閉環幀全部刪除for(int i=0; i<nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();mpCurrentKF->SetErase();return false;}
}

9.4 閉環矯正:?CorrectLoop()

函數LoopClosing::CorrectLoop()的主要流程:

Sim3位姿傳播:
將Sim3位姿傳播到局部關鍵幀組上.
將Sim3位姿傳播到局部地圖點上.
地圖點融合:
將閉環關鍵幀組地圖點投影到當前關鍵幀上.
將閉環關鍵幀組地圖點投影到局部關鍵幀組上.
BA優化
本質圖BA優化: 優化所有地圖點和關鍵幀位姿,基于本質圖.
全局BA優化: 優化所有地圖點和關鍵幀位姿,基于地圖點到關鍵幀的投影關系.
?

/*** @brief 閉環矯正* 1. 通過求解的Sim3以及相對姿態關系，調整與當前幀相連的關鍵幀位姿以及這些關鍵幀觀測到的地圖點位置（相連關鍵幀---當前幀） * 2. 將閉環幀以及閉環幀相連的關鍵幀的地圖點和與當前幀相連的關鍵幀的點進行匹配（當前幀+相連關鍵幀---閉環幀+相連關鍵幀）     * 3. 通過MapPoints的匹配關系更新這些幀之間的連接關系，即更新covisibility graph                                      * 4. 對Essential Graph（Pose Graph）進行優化，MapPoints的位置則根據優化后的位姿做相對應的調整                        * 5. 創建線程進行全局Bundle Adjustment*/
void LoopClosing::CorrectLoop()
{cout << "Loop detected!" << endl;// Step 0：結束局部地圖線程、全局BA，為閉環矯正做準備// Step 1：根據共視關系更新當前幀與其它關鍵幀之間的連接// Step 2：通過位姿傳播，得到Sim3優化后，與當前幀相連的關鍵幀的位姿，以及它們的MapPoints// Step 3：檢查當前幀的MapPoints與閉環匹配幀的MapPoints是否存在沖突，對沖突的MapPoints進行替換或填補// Step 4：通過將閉環時相連關鍵幀的mvpLoopMapPoints投影到這些關鍵幀中，進行MapPoints檢查與替換// Step 5：更新當前關鍵幀之間的共視相連關系，得到因閉環時MapPoints融合而新得到的連接關系// Step 6：進行EssentialGraph優化，LoopConnections是形成閉環后新生成的連接關系，不包括步驟7中當前幀與閉環匹配幀之間的連接關系// Step 7：添加當前幀與閉環匹配幀之間的邊（這個連接關系不優化）// Step 8：新建一個線程用于全局BA優化// g2oSic： 當前關鍵幀 mpCurrentKF 到其共視關鍵幀 pKFi 的Sim3 相對變換// mg2oScw: 世界坐標系到當前關鍵幀的 Sim3 變換// g2oCorrectedSiw：世界坐標系到當前關鍵幀共視關鍵幀的Sim3 變換// Send a stop signal to Local Mapping// Avoid new keyframes are inserted while correcting the loop// Step 0：結束局部地圖線程、全局BA，為閉環矯正做準備// 請求局部地圖停止，防止在回環矯正時局部地圖線程中InsertKeyFrame函數插入新的關鍵幀mpLocalMapper->RequestStop();if(isRunningGBA()){// 如果有全局BA在運行，終止掉，迎接新的全局BAunique_lock<mutex> lock(mMutexGBA);mbStopGBA = true;// 記錄全局BA次數mnFullBAIdx++;if(mpThreadGBA){// 停止全局BA線程mpThreadGBA->detach();delete mpThreadGBA;}}// Wait until Local Mapping has effectively stopped// 一直等到局部地圖線程結束再繼續while(!mpLocalMapper->isStopped()){std::this_thread::sleep_for(std::chrono::milliseconds(1));}// Ensure current keyframe is updated// Step 1：根據共視關系更新當前關鍵幀與其它關鍵幀之間的連接關系// 因為之前閉環檢測、計算Sim3中改變了該關鍵幀的地圖點，所以需要更新mpCurrentKF->UpdateConnections();// Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation// Step 2：通過位姿傳播，得到Sim3優化后，與當前幀相連的關鍵幀的位姿，以及它們的地圖點// 當前幀與世界坐標系之間的Sim變換在ComputeSim3函數中已經確定并優化，// 通過相對位姿關系，可以確定這些相連的關鍵幀與世界坐標系之間的Sim3變換// 取出當前關鍵幀及其共視關鍵幀，稱為“當前關鍵幀組”mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();mvpCurrentConnectedKFs.push_back(mpCurrentKF);// CorrectedSim3：存放閉環g2o優化后當前關鍵幀的共視關鍵幀的世界坐標系下Sim3 變換// NonCorrectedSim3：存放沒有矯正的當前關鍵幀的共視關鍵幀的世界坐標系下Sim3 變換KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;// 先將mpCurrentKF的Sim3變換存入，認為是準的，所以固定不動CorrectedSim3[mpCurrentKF]=mg2oScw;// 當前關鍵幀到世界坐標系下的變換矩陣cv::Mat Twc = mpCurrentKF->GetPoseInverse();// 對地圖點操作{// Get Map Mutex// 鎖定地圖點unique_lock<mutex> lock(mpMap->mMutexMapUpdate);// Step 2.1：通過mg2oScw（認為是準的）來進行位姿傳播，得到當前關鍵幀的共視關鍵幀的世界坐標系下Sim3 位姿// 遍歷"當前關鍵幀組""for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++){KeyFrame* pKFi = *vit;cv::Mat Tiw = pKFi->GetPose();if(pKFi!=mpCurrentKF)      //跳過當前關鍵幀，因為當前關鍵幀的位姿已經在前面優化過了，在這里是參考基準{// 得到當前關鍵幀 mpCurrentKF 到其共視關鍵幀 pKFi 的相對變換cv::Mat Tic = Tiw*Twc;cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);cv::Mat tic = Tic.rowRange(0,3).col(3);// g2oSic：當前關鍵幀 mpCurrentKF 到其共視關鍵幀 pKFi 的Sim3 相對變換// 這里是non-correct, 所以scale=1.0g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);// 當前幀的位姿固定不動，其它的關鍵幀根據相對關系得到Sim3調整的位姿g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;// Pose corrected with the Sim3 of the loop closure// 存放閉環g2o優化后當前關鍵幀的共視關鍵幀的Sim3 位姿CorrectedSim3[pKFi]=g2oCorrectedSiw;}cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);cv::Mat tiw = Tiw.rowRange(0,3).col(3);g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);// Pose without correction// 存放沒有矯正的當前關鍵幀的共視關鍵幀的Sim3變換NonCorrectedSim3[pKFi]=g2oSiw;}// Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop// Step 2.2：得到矯正的當前關鍵幀的共視關鍵幀位姿后，修正這些共視關鍵幀的地圖點// 遍歷待矯正的共視關鍵幀（不包括當前關鍵幀）for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++){// 取出當前關鍵幀連接關鍵幀KeyFrame* pKFi = mit->first;// 取出經過位姿傳播后的Sim3變換g2o::Sim3 g2oCorrectedSiw = mit->second;g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();// 取出未經過位姿傳播的Sim3變換g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();// 遍歷待矯正共視關鍵幀中的每一個地圖點for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++){MapPoint* pMPi = vpMPsi[iMP];// 跳過無效的地圖點if(!pMPi)continue;if(pMPi->isBad())continue;// 標記，防止重復矯正if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) continue;// 矯正過程本質上也是基于當前關鍵幀的優化后的位姿展開的// Project with non-corrected pose and project back with corrected pose// 將該未校正的eigP3Dw先從世界坐標系映射到未校正的pKFi相機坐標系，然后再反映射到校正后的世界坐標系下cv::Mat P3Dw = pMPi->GetWorldPos();// 地圖點世界坐標系下坐標Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);// map(P) 內部做了相似變換 s*R*P +t  // 下面變換是：eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ worldEigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);pMPi->SetWorldPos(cvCorrectedP3Dw);// 記錄矯正該地圖點的關鍵幀id，防止重復pMPi->mnCorrectedByKF = mpCurrentKF->mnId;// 記錄該地圖點所在的關鍵幀idpMPi->mnCorrectedReference = pKFi->mnId;// 因為地圖點更新了，需要更新其平均觀測方向以及觀測距離范圍pMPi->UpdateNormalAndDepth();}// Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)// Step 2.3：將共視關鍵幀的Sim3轉換為SE3，根據更新的Sim3，更新關鍵幀的位姿// 其實是現在已經有了更新后的關鍵幀組中關鍵幀的位姿,但是在上面的操作時只是暫時存儲到了 KeyFrameAndPose 類型的變量中,還沒有寫回到關鍵幀對象中// 調用toRotationMatrix 可以自動歸一化旋轉矩陣Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); Eigen::Vector3d eigt = g2oCorrectedSiw.translation();                  double s = g2oCorrectedSiw.scale();// 平移向量中包含有尺度信息，還需要用尺度歸一化eigt *=(1./s); cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);// 設置矯正后的新的posepKFi->SetPose(correctedTiw);// Make sure connections are updated// Step 2.4：根據共視關系更新當前幀與其它關鍵幀之間的連接// 地圖點的位置改變了,可能會引起共視關系\權值的改變 pKFi->UpdateConnections();}// Start Loop Fusion// Update matched map points and replace if duplicated// Step 3：檢查當前幀的地圖點與經過閉環匹配后該幀的地圖點是否存在沖突，對沖突的進行替換或填補// mvpCurrentMatchedPoints 是當前關鍵幀和閉環關鍵幀組的所有地圖點進行投影得到的匹配點for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++){if(mvpCurrentMatchedPoints[i]){//取出同一個索引對應的兩種地圖點，決定是否要替換// 匹配投影得到的地圖點MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];// 原來的地圖點MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i); if(pCurMP)// 如果有重復的MapPoint，則用匹配的地圖點代替現有的// 因為匹配的地圖點是經過一系列操作后比較精確的，現有的地圖點很可能有累計誤差pCurMP->Replace(pLoopMP);else{// 如果當前幀沒有該MapPoint，則直接添加mpCurrentKF->AddMapPoint(pLoopMP,i);pLoopMP->AddObservation(mpCurrentKF,i);pLoopMP->ComputeDistinctiveDescriptors();}}} }// Project MapPoints observed in the neighborhood of the loop keyframe// into the current keyframe and neighbors using corrected poses.// Fuse duplications.// Step 4：將閉環相連關鍵幀組mvpLoopMapPoints 投影到當前關鍵幀組中，進行匹配，融合，新增或替換當前關鍵幀組中KF的地圖點// 因為 閉環相連關鍵幀組mvpLoopMapPoints 在地圖中時間比較久經歷了多次優化，認為是準確的// 而當前關鍵幀組中的關鍵幀的地圖點是最近新計算的，可能有累積誤差// CorrectedSim3：存放矯正后當前關鍵幀的共視關鍵幀，及其世界坐標系下Sim3 變換SearchAndFuse(CorrectedSim3);// After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop// Step 5：更新當前關鍵幀組之間的兩級共視相連關系，得到因閉環時地圖點融合而新得到的連接關系// LoopConnections：存儲因為閉環地圖點調整而新生成的連接關系map<KeyFrame*, set<KeyFrame*> > LoopConnections;// Step 5.1：遍歷當前幀相連關鍵幀組（一級相連）for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++){KeyFrame* pKFi = *vit;// Step 5.2：得到與當前幀相連關鍵幀的相連關鍵幀（二級相連）vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();// Update connections. Detect new links.// Step 5.3：更新一級相連關鍵幀的連接關系(會把當前關鍵幀添加進去,因為地圖點已經更新和替換了)pKFi->UpdateConnections();// Step 5.4：取出該幀更新后的連接關系LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();// Step 5.5：從連接關系中去除閉環之前的二級連接關系，剩下的連接就是由閉環得到的連接關系for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++){LoopConnections[pKFi].erase(*vit_prev);}// Step 5.6：從連接關系中去除閉環之前的一級連接關系，剩下的連接就是由閉環得到的連接關系for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++){LoopConnections[pKFi].erase(*vit2);}}// Optimize graph// Step 6：進行本質圖優化，優化本質圖中所有關鍵幀的位姿和地圖點// LoopConnections是形成閉環后新生成的連接關系，不包括步驟7中當前幀與閉環匹配幀之間的連接關系Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);// Add loop edge// Step 7：添加當前幀與閉環匹配幀之間的邊（這個連接關系不優化）// 它在下一次的本質圖優化里面使用mpMatchedKF->AddLoopEdge(mpCurrentKF);mpCurrentKF->AddLoopEdge(mpMatchedKF);// Launch a new thread to perform Global Bundle Adjustment// Step 8：新建一個線程用于全局BA優化// OptimizeEssentialGraph只是優化了一些主要關鍵幀的位姿，這里進行全局BA可以全局優化所有位姿和MapPointsmbRunningGBA = true;mbFinishedGBA = false;mbStopGBA = false;mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);// Loop closed. Release Local Mapping.mpLocalMapper->Release();    cout << "Loop Closed!" << endl;mLastLoopKFid = mpCurrentKF->mnId;
}

10. ORB-SLAM2代碼詳解十大trick

10.1. 關鍵幀與關鍵點的刪除

由于關鍵幀與關鍵點的作用各個地方都要用到，所以對關鍵幀和地圖點的操作就屬于一種“重操作”，時間消耗很嚴重，這里作者采用了很巧妙的辦法來處理，以防止系統卡頓或者加鎖計算對系統的數據資源的共享。總的來說是采用一種先標記后處理的方式，標志該地圖點或者關鍵幀社會性死亡。當然這帶來的副作用就是每一個地圖點或者關鍵幀都多了一個狀態值，每次訪問之前都需要判斷關鍵幀或者地圖點的狀態以判斷該地圖點或者關鍵幀的狀態還正常。

10.2 ORB特征點提取過程中的超像素處理
首先作者通過很巧妙的方法（極大值抑制，分區處理，遞歸等方法）獲取了均勻分布的特征點。隨后在極線方向進行搜索，獲取對應的匹配點。接著在對應的匹配點的位置左右倆個側，取三個點，根據極限上三個點的位置擬合出一條高斯曲線，獲取擬合的高斯曲線的最值所在的點，也即最終的匹配點在極限上的坐標

10.3 最小生成樹的維護
10.4 不同高斯金字塔下的視差與距離的約束關系的增加

?