ORB-SLAM2中生成金字塔提取FAST角點和計算BRIEF描述子

//這個是類ORBextractor的帶參構造函數，并且使用初始化列表對該類中的這5個變量賦值
ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,int _iniThFAST, int _minThFAST):nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),iniThFAST(_iniThFAST), minThFAST(_minThFAST)
{//mvScaleFactor是用來存儲金字塔中每層圖像對應的尺度因子的vector變量//所以他的大小就是金字塔的層數mvScaleFactor.resize(nlevels);//mvLevelSigma2是該層尺度因子的平方，有點表示面積的意思，具體含義等遇到了再解釋mvLevelSigma2.resize(nlevels);//將vector中的第一個元素值初始化為1.mvScaleFactor[0]=1.0f;mvLevelSigma2[0]=1.0f;//計算金字塔中每一層圖像對應的尺度因子和尺度因子的平方。//可以發現金字塔中第0層的尺度因子是1,然后每向上高一層，圖像的尺度因子是在上一層圖像的尺度因子 //上乘以scaleFactor，在本工程下該值為1.2//1   1*1.2   1*1.2*1.2    1*1.2*1.2*1.2   ...for(int i=1; i<nlevels; i++){mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i];}//如果說上面是正向的尺度，那么下面的就是逆向尺度了，是正向尺度的倒數mvInvScaleFactor.resize(nlevels);mvInvLevelSigma2.resize(nlevels);for(int i=0; i<nlevels; i++){mvInvScaleFactor[i]=1.0f/mvScaleFactor[i];mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];}//mvImagePyramid是一個存儲金字塔圖像的vector，vector中的每一個元素是用Mat數據類型表示的圖        //像, std::vector<cv::Mat> mvImagePyramid;mvImagePyramid.resize(nlevels);//mnFeaturesPerLevel是一個存儲金字塔中每層圖像應該提取的特征點的個數//std::vector<int> mnFeaturesPerLevel;mnFeaturesPerLevel.resize(nlevels);//那在這里factor = 1/1.2;float factor = 1.0f / scaleFactor;//nDesiredFeaturesPerScale是根據總的要在整幅圖像中提取的特征點數nFeatures(在這里是1000)//還有金字塔的層數來計算每層圖像上應該提取的特征點的個數。//根據下面的程序可以得知nDesiredFeaturesPerScale是第0層圖像上應該提取的特征點的個數float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));int sumFeatures = 0;for( int level = 0; level < nlevels-1; level++ ){//那么mnFeaturesPerLevel[0]就是上面計算出來的這個值嘍mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);//由于四舍五入的原因將實際計算出的數值加到一起與預設的1000比較sumFeatures += mnFeaturesPerLevel[level];//層數越高則需要提取的特征的個數就越少，并且相鄰層圖像上提取的特征點的個數存在1.2倍//關系//這里是我計算的結果//217+180+150+125+104+87+72+60 = 995nDesiredFeaturesPerScale *= factor;}//看到這里就會知道上面為啥要把計算出來的特征點個數求和的原因了，就是來決定最后//一層圖像應該提取的特征的個數了，根據計算最后一層要提取60個點，那現在就得提取65個//才能達到總共提取1000個點的要求。mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);//下面這一塊是為了提取特征點做準備了，等后面再講解。const int npoints = 512;const Point* pattern0 = (const Point*)bit_pattern_31_;std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));//This is for orientation// pre-compute the end of a row in a circular patchumax.resize(HALF_PATCH_SIZE + 1);int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;for (v = 0; v <= vmax; ++v)umax[v] = cvRound(sqrt(hp2 - v * v));// Make sure we are symmetricfor (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v){while (umax[v0] == umax[v0 + 1])++v0;umax[v] = v0;++v0;}
}//下面這個函數實現對輸入的圖像計算圖像金字塔的任務
void ORBextractor::ComputePyramid(cv::Mat image)
{//nlevels = 8for (int level = 0; level < nlevels; ++level){//在構造函數中已經提前定義好了每一層圖像對應的反向尺度因子float scale = mvInvScaleFactor[level];//不同的level，scale的值不同所以就算出了每一層上的圖像的大小Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));//為新生成的圖像加上邊界Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);//根據上面的計算的尺度來創建一幅新的圖像， Mat類型//type()是Mat類中的一個成員函數，返回數據類型Mat temp(wholeSize, image.type()), masktemp;//mvImagePyramid是用來存儲每一層圖像的vector變量，為他的每一個元素設置特定大小的圖像//mvImagePyramid[0]中存儲的是原圖像//通過Rect定義temp圖像的左上側起點和右下側終點mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));// Compute the resized imageif( level != 0 ){//mvImagePyramid[1]開始，都是由上一層的圖像的尺寸得到// dsize = Size(round(fx*src.cols), round(fy*src.rows))//dsize是輸出圖像的大小，按照上面的計算公式計算得到了已經//resize(InputArry src, Output dst, Size dsize, double fx = 0, double fy = 0, //int interpolation = INTER_LINEAR)resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);//將設置出的圖像分別拷貝到相應的層上去copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,BORDER_REFLECT_101+BORDER_ISOLATED);            }else{copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,BORDER_REFLECT_101);            }}}//函數的三要素是：函數名稱，函數參數， 函數返回值
//ComputeKeyPointsOctTree是類ORBextractor的成員函數
//參數是vector類型的引用變量allKeypoints.
//返回值是void類型
//在參數文件TUM1.yaml下預定義了一些變量的值
//ORBextractor. nFeatures: 1000
//ORBextractor. scaleFactor: 1.2
//ORBextractor. nlevels: 8
//ORBextractor. iniThFAST: 20
//ORBextractor. minThFAST: 7
//在ORBextractor.h中用帶參構造函數來初始化類ORBextractor中的相應的變量
//在類ORBextractor中還有一串變量
//std::vector<float> mvScaleFactor;
//std::vector<float> mvInvScaleFactor;
//std::vector<float> mvlevelSifma2;
//std::vector<float> mvInvLevelSigma2;
//如果說第一組變量是金字塔中某一層圖像的屬性，那么第二組是成員變量是一幅圖像的屬性。
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{//通過vector中的resize函數來重新將vector變量allKeypoints的大小設置為nlevels.//nlevels也就是圖像金字塔中圖像的層數allKeypoints.resize(nlevels);const float W = 30;//對金字塔中的每一層圖像進行一系列的操作for (int level = 0; level < nlevels; ++level){//EDGE_THRESHOLD=19//minBorderX, minBorderY, maxBorderX, maxBorderY四個變量一起設定了用于提取特征的區域const int minBorderX = EDGE_THRESHOLD-3;const int minBorderY = minBorderX;const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;//定義變量vToDistributeKeys存儲從每一層圖像中提取出來的特征。//vector中存儲的數據類型是在opencv中定義的KeyPoint類vector<cv::KeyPoint> vToDistributeKeys;//reserve：分配空間，更改capacity但是不改變size 預留空間//resize：分配空間，同時改變capacity和sizevToDistributeKeys.reserve(nfeatures*10);//丈量了可以用來進行操作的“場地”大小const float width = (maxBorderX-minBorderX);const float height = (maxBorderY-minBorderY);//預將圖像劃分為30*30的網狀//計算每個小格子的長和寬各占多少個像素const int nCols = width/W;const int nRows = height/W;///計算最終長和寬被分成了多少個小格子cellconst int wCell = ceil(width/nCols);const int hCell = ceil(height/nRows);//通過控制i來縱向遍歷每一個cellfor(int i=0; i<nRows; i++){const float iniY =minBorderY+i*hCell;//這里maxY相當于給當前遍歷的這個cell加上一個外框float maxY = iniY+hCell+6;if(iniY>=maxBorderY-3)continue;if(maxY>maxBorderY)maxY = maxBorderY;//通過控制j來橫向遍歷每一個cell.for(int j=0; j<nCols; j++){const float iniX =minBorderX+j*wCell;float maxX = iniX+wCell+6;if(iniX>=maxBorderX-6)continue;if(maxX>maxBorderX)maxX = maxBorderX;//通過上面的兩個for循環就可以完成遍歷該層圖像中所有的cell.//vKeysCell用來盛放該cell中提取的特征點vector<cv::KeyPoint> vKeysCell;//變量i和j的組合控制，當遍歷到（i, j）個cell時，就提取這個cell下的FAST角點//如下是opencv中FAST函數的原型//輸入圖像，輸出提取的特征點， 選取特征點的閾值///FAST( InputArray image, CV_OUT vector<KeyPoint>& keypoints,/// int threshold, bool nonmaxSuppression=true );FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),vKeysCell,iniThFAST,true);if(vKeysCell.empty()){///如果按照上面的方法在這個cell中提取不到特征點，那么就放低要求，//用minThFAST閾值來提取FAST角點FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),vKeysCell,minThFAST,true);}if(!vKeysCell.empty()){//如果已經提取到關鍵點，那么就遍歷這些所有提取的關鍵點for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++){
///迭代器vit就相當于一個指向vector中存儲的KeyPoint的指針，通過*vit就可以獲取指針所指向的的
//特定的點//pt表示KeyPoint的位置屬性//KeyPoint是opencv中的一個類,pt是該類中的一個屬性，獲取獲取關鍵點的坐標
//因為單純的(*vit).pt.x和(*vit).pt.y表示在當前cell下的坐標，還要轉化為在可提取特征范圍內的坐標(*vit).pt.x+=j*wCell;(*vit).pt.y+=i*hCell;//把從每個cell中提取的特征點都存儲到vector變量vToDistributeKeys中去vToDistributeKeys.push_back(*vit);}}}}
//截止到這里已經將該層圖像中的所有cell遍歷結束并且，將提取的所有的特征點都已經存儲到vector
//變量vToDistributeKeys中去了//vector<vector<KeyPoint> >& allKeypoints//allKeypoints是一個用來存儲vector的vector//allKeypoints的大小是金字塔的層數nlevels//allKeypoints[level]是一個對應于每層圖像上提取的特征點的vector//allKeypoints[level].size也就是在該層上要提取的特征點的個數vector<KeyPoint> & keypoints = allKeypoints[level];//keypoints的預留內存是每幅圖像上所有要提取的特征數。keypoints.reserve(nfeatures);所有提取的關鍵點，提取的范圍，是從哪一層上提取的特征///下面這部分是將提取的關鍵點進行八叉樹優化keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);///PATCH_SIZE指代什么呢level=0表示原圖像，隨著層數的增加圖像越來越小，那么在每幅圖像上提取的特征個數
//也會相應的減少// PATCH_SIZE = 31.//vector變量mvScaleFactor中存儲了一幅圖像對應的一個金字塔中所有層圖像的尺度因子//不同層圖像的尺度因子不同，那么在該層中提取的特征點所對應的有效區域就不同。const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];//nkps表示keypoints中的特征點的個數const int nkps = keypoints.size();for(int i=0; i<nkps ; i++){///遍歷在該層圖像上提取的所有的特征點，在這些特征點坐標上都加上整幅圖像的邊界信息就可以
//得到關鍵點在整幅圖像中的坐標keypoints[i].pt.x+=minBorderX;keypoints[i].pt.y+=minBorderY;//類KeyPoint一個成員變量octave用來存儲該特征點是在哪一層圖像上提取得到的。//我們之后在使用某一個特征點的時候就可以直接通過特的這個屬性來知道他是從
//哪一層圖像上提取出來的。keypoints[i].octave=level;///這每一個關鍵點的size該如何理解呢，大概是根據近大遠小的原理來計算的。在不同層圖像上提取的點所表征的面積范圍不同。keypoints[i].size = scaledPatchSize;}}// 遍歷每一層圖像以及在該層上提取的特征點，計算每個特征點的方向。for (int level = 0; level < nlevels; ++level)computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}

總結：

如何更以更清晰的思路來分析和學習ORB-SLAM呢，我認為一種好的方式就是編譯器如何利用C++這種工具來創建和維護該工程下一些變量的。如用來檢測閉環和重定位，local BA，full BA，用來描述系統中關鍵幀之間的共視關系的covisibility graph，以及由covisibility graph得到的 Enssential graph進行位姿圖優化。以上主要是利用C++下的強大的STL中的容器的概念來創建和存儲這些關系如 std::map<KeyFrame* int> mConnectedKeyFrameWeights; 用map來存儲了與當前幀有共視關系的關鍵幀以及共視的地圖點的個數。

要知道幀與幀的聯系是通過地圖點建立起來的，并且在創建特征點的時候為這個地圖點添加observation屬性，用map來存儲這個地圖點可以被那個關鍵幀觀測到，以及對應于該圖像中的特征點的index.

而對于新進來的一幅圖像我們要對她進行預處理，創建金字塔，提取FAST角點和計算BRIEF描述子，

用變量std::vector<cv::Mat> mvImagePyramid 來存儲計算出來的金字塔中的每一層圖像

用變量std::vector<std::vector>allKeypoints來存儲金字塔中每一層圖像中提取的特征點。

用變量std::vector<float> mvScaleFactor來存儲金字塔中每一層圖像對應的尺度因子

用變量std::vector<int> mnFeaturesPerLevel來存儲金字塔中每一幅圖像應該提取的特征點的個數

最后這兩個變量都是在類ORBextractor中的構造函數中計算初始化的。

輸入一幅圖像首先要計算8層金字塔，然后是針對于金字塔中的每一層圖像劃分為grid來提取特征點，然后將這些特征點用八叉樹進行優化，然后在計算描述子，和觀測方向。