ORB-SLAM2源碼解析（一）：ORB算法
目錄
一、前言
二、ORB算法原理
三、ORB－SLAM2中ORB算法代碼注釋分析
四、與opencv中ORB對比測試
五、總結
六、參考
一、前言
目前在學習ORB-SLAM2中，看了一段時間卻僅僅學到皮毛，于是決定深入代碼，逐漸蠶食各個部分，今天從ORB-SLAM2中使用的ORB算法開始。這裡記錄我的學習過程，同時增加了一些個人了解注釋。
二、ORB算法原理
ORB算法原理主要來自于文章《ORB an efficient alternative to SIFT or SURF》。在參考文獻（１）中對該文獻進行了大概的翻譯，這裡不在再重複，總結來說ORB是基于FAST角點和BRIEF描述子，準确來說也加入了Harris角點的角點評分方法，不過在ORB－SLAM2代碼中并沒用到，ORB－SLAM2使用的它的貢獻在與：
- 使用灰階質心法計算了角點的方向，為描述子增加了旋轉不變性；
- 使用Harris角點的角點評分方法為FAST角點增加角點評分，（在ｏｐｅｎｃｖ可以選擇使用Harris角點的角點評分方法還是使用Harris角點的角點評分方法）。
如果你對BRIEF特征點描述子、FAST和Harris角點沒有基礎的認識，參考文章２、３、４。
總的原理以文章１為主，接下來進入代碼分析，這裡給出代碼的完全注釋，具體參考了文章５＼６＼７＼８，有興趣的可以看一看。
三、ORB－SLAM2中ORB算法代碼注釋分析
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vector>
#include <iterator>
#include "ORBextractor.h"
#include <iostream>
using namespace cv;
using namespace std;
namespace ORB_SLAM2
{
const int PATCH_SIZE = 31;//用來做灰階質心的圓圓的直徑
const int HALF_PATCH_SIZE = 15;//圓的半徑１５＋１５＋１（１為中心點的那個像素）
const int EDGE_THRESHOLD = 19; //邊界門檻值
//灰階質心法（IC）計算特征的旋轉
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
{
int m_01 = 0, m_10 = 0;
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x)); //cvRound 傳回跟參數最接近的整數值；
//我們要在一個圓域中算出m10（u）和m01(v)，計算步驟是先算出v=0的m10，然後在平行于x軸算出m10和m01，一次計算相當于圖像中的同個顔色的兩個line。參考文章７
// Treat the center line differently, v=0 橫坐标：-15-----+15
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
m_10 += u * center[u];
// Go line by line in the circuI853lar patch
int step = (int)image.step1(); //opencv中概念，計算每行的元素個數
for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
{
// Proceed over the two lines
int v_sum = 0;
int d = u_max[v];
for (int u = -d; u <= d; ++u)
{
int val_plus = center[u + v*step], val_minus = center[u - v*step];
v_sum += (val_plus - val_minus);
m_10 += u * (val_plus + val_minus);
}
m_01 += v * v_sum;
}
//傳回計算的角度
return fastAtan2((float)m_01, (float)m_10);
}
//弧度制與角度的轉換
const float factorPI = (float)(CV_PI/180.f);
//計算描述子
static void computeOrbDescriptor(const KeyPoint& kpt,
const Mat& img, const Point* pattern,
uchar* desc)
{
float angle = (float)kpt.angle*factorPI;
float a = (float)cos(angle), b = (float)sin(angle);
const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
const int step = (int)img.step;
#define GET_VALUE(idx) \
center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
cvRound(pattern[idx].x*a - pattern[idx].y*b)]//這裡使用旋轉不變性
for (int i = 0; i < 32; ++i, pattern += 16)
{
。。。
}
#undef GET_VALUE//取消宏定義
}
//這裡的點是經過機器學習訓練過的點，用來做二進制描述子提取，具體參考文章２BRIEF描述子
static int bit_pattern_31_[256*4] =
{
。。。。
};
ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
int _iniThFAST, int _minThFAST):
nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
iniThFAST(_iniThFAST), minThFAST(_minThFAST)
{
/***********确定每一層的特征點數，采用等比數列**************/
//定義每一層的尺度和逆尺度
mvScaleFactor.resize(nlevels);
mvLevelSigma2.resize(nlevels);
mvScaleFactor[0]=1.0f;
mvLevelSigma2[0]=1.0f;
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.resize(nlevels);
mnFeaturesPerLevel.resize(nlevels);
float factor = 1.0f / scaleFactor;
//第一層特征點數，以後每一層成等比數列，利用的是等比數列求和公式
float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
//所有層數的特征點數量加起來是nfeatures
int sumFeatures = 0;
for( int level = 0; level < nlevels-1; level++ )
{
mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale); //取整
sumFeatures += mnFeaturesPerLevel[level];
nDesiredFeaturesPerScale *= factor;
}
mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);//保證特征點數
//複制訓練的模闆
const int npoints = 512;//256對點＊２
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 patch
//計算方向時，每個v對應的最大的u坐标
umax.resize(HALF_PATCH_SIZE + 1);
// 将v坐标劃分為兩部分進行計算，主要為了確定計算特征主方向的時候，x,y方向對稱
int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);//cvFloor含義是取不大于參數的最大整數值
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2); //cvCeil含義是取不小于參數的最小整數值
//利用勾股定理計算坐标
const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE; //patch圓半徑的平方
for (v = 0; v <= vmax; ++v)
umax[v] = cvRound(sqrt(hp2 - v * v)); //每一個v坐标，最大的U坐标
// Make sure we are symmetric 確定是圓
for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
{
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}
}
//計算每個關鍵點的角度
static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
{
for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
{
keypoint->angle = IC_Angle(image, keypoint->pt, umax);
}
}
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
{
const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);
//Define boundaries of childs
n1.UL = UL;
n1.UR = cv::Point2i(UL.x+halfX,UL.y);
n1.BL = cv::Point2i(UL.x,UL.y+halfY);
n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);
n1.vKeys.reserve(vKeys.size());
n2.UL = n1.UR;
n2.UR = UR;
n2.BL = n1.BR;
n2.BR = cv::Point2i(UR.x,UL.y+halfY);
n2.vKeys.reserve(vKeys.size());
n3.UL = n1.BL;
n3.UR = n1.BR;
n3.BL = BL;
n3.BR = cv::Point2i(n1.BR.x,BL.y);
n3.vKeys.reserve(vKeys.size());
n4.UL = n3.UR;
n4.UR = n2.BR;
n4.BL = n3.BR;
n4.BR = BR;
n4.vKeys.reserve(vKeys.size());
//Associate points to childs
for(size_t i=0;i<vKeys.size();i++)
{
const cv::KeyPoint &kp = vKeys[i];
if(kp.pt.x<n1.UR.x)
{
if(kp.pt.y<n1.BR.y)
n1.vKeys.push_back(kp);
else
n3.vKeys.push_back(kp);
}
else if(kp.pt.y<n1.BR.y)
n2.vKeys.push_back(kp);
else
n4.vKeys.push_back(kp);
}
if(n1.vKeys.size()==1)
n1.bNoMore = true;
if(n2.vKeys.size()==1)
n2.bNoMore = true;
if(n3.vKeys.size()==1)
n3.bNoMore = true;
if(n4.vKeys.size()==1)
n4.bNoMore = true;
}
//計算fast特征點并進行篩選
//先用(maxX-minX)/(maxY-minY)來确定四叉數有幾個初始節點，這裡有 bug，如果輸入的是一張寬高比小于0.5 的圖像，
//nIni 計算得到 0，下一步計算 hX 會報錯，例如round(640/480)=1,是以隻有一個初始節點，(UL,UR,BL,BR)就會分布到被裁掉邊界後的圖像的四個角。
//把所有的關鍵點配置設定給屬于它的節點，當節點所配置設定到的關鍵點的個數為1時就不再進行分裂，當節點沒有配置設定到關鍵點時就删除此節點。
//再根據興趣點分布,利用四叉樹方法對圖像進行劃分區域，當bFinish的值為true時就不再進行區域劃分，首先對目前的區域進行劃分，
//把每次劃分得到的有關鍵點的子區域設為新的節點，将nToExpand參數加一，并插入到節點清單的前邊，删除掉其父節點。
//隻要新節點中的關鍵點的個數超過一個，就繼續劃分，繼續插入清單前面，繼續删除父節點，直到劃分的子區域中的關鍵點的個數是一個，
//然後疊代器加以移動到下一個節點，繼續劃分區域。
//當劃分的區域即節點的個數大于關鍵點的個數或者分裂過程沒有增加節點的個數時就将bFinish的值設為true，不再進行劃分。
//如果以上條件沒有滿足，但是滿足((int)lNodes.size()+nToExpand*3)>N，則說明将所有節點再分裂一次可以達到要求。
//vSizeAndPointerToNode 是前面分裂出來的子節點（n1, n2, n3, n4）中可以分裂的節點。
//按照它們特征點的排序，先從特征點多的開始分裂，分裂的結果繼續存儲在 lNodes 中。每分裂一個節點都會進行一次判斷，
//如果 lNodes 中的節點數量大于所需要的特征點數量，退出整個 while(!bFinish) 循環，如果進行了一次分裂，
//并沒有增加節點數量，不玩了，退出整個 while(!bFinish) 循環。取出每一個節點(每個區域)對應的最大響應點，即我們确定的特征點。
vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
{
// Compute how many initial nodes 計算有多少初始節點
const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY)); //round四舍五入取整，水準劃分格子的數量
const float hX = static_cast<float>(maxX-minX)/nIni; //水準劃分格子的寬度
list<ExtractorNode> lNodes;
vector<ExtractorNode*> vpIniNodes;
vpIniNodes.resize(nIni);
for(int i=0; i<nIni; i++)
{
ExtractorNode ni;
ni.UL = cv::Point2i(hX*static_cast<float>(i),0);
ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);
ni.BL = cv::Point2i(ni.UL.x,maxY-minY);
ni.BR = cv::Point2i(ni.UR.x,maxY-minY);
ni.vKeys.reserve(vToDistributeKeys.size());
lNodes.push_back(ni);
vpIniNodes[i] = &lNodes.back();
}
//Associate points to childs
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
const cv::KeyPoint &kp = vToDistributeKeys[i];
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}
list<ExtractorNode>::iterator lit = lNodes.begin();
while(lit!=lNodes.end())
{
if(lit->vKeys.size()==1)
{
lit->bNoMore=true;
lit++;
}
else if(lit->vKeys.empty())
lit = lNodes.erase(lit);
else
lit++;
}
bool bFinish = false;
int iteration = 0;
vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
vSizeAndPointerToNode.reserve(lNodes.size()*4);
// 根據興趣點分布,利用N叉樹方法對圖像進行劃分區域
while(!bFinish)
{
iteration++;
int prevSize = lNodes.size();
lit = lNodes.begin();
int nToExpand = 0;
vSizeAndPointerToNode.clear();
// 将目前的子區域經行劃分
while(lit!=lNodes.end())
{
if(lit->bNoMore)
{
// If node only contains one point do not subdivide and continue
lit++;
continue;
}
else
{
// If more than one point, subdivide
ExtractorNode n1,n2,n3,n4;
lit->DivideNode(n1,n2,n3,n4); // 再細分成四個子區域
// Add childs if they contain points
if(n1.vKeys.size()>0)
{
lNodes.push_front(n1);
if(n1.vKeys.size()>1)
{
nToExpand++;
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n2.vKeys.size()>0)
{
lNodes.push_front(n2);
if(n2.vKeys.size()>1)
{
nToExpand++;
vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n3.vKeys.size()>0)
{
lNodes.push_front(n3);
if(n3.vKeys.size()>1)
{
nToExpand++;
vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n4.vKeys.size()>0)
{
lNodes.push_front(n4);
if(n4.vKeys.size()>1)
{
nToExpand++;
vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
lit=lNodes.erase(lit);
continue;
}
}
// Finish if there are more nodes than required features
// or all nodes contain just one point
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
{
bFinish = true;
}
// 當再劃分之後所有的Node數大于要求數目時
else if(((int)lNodes.size()+nToExpand*3)>N)
{
while(!bFinish)
{
prevSize = lNodes.size();
vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
vSizeAndPointerToNode.clear();
// 對需要劃分的部分進行排序, 即對興趣點數較多的區域進行劃分
sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
{
ExtractorNode n1,n2,n3,n4;
vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);
// Add childs if they contain points
if(n1.vKeys.size()>0)
{
lNodes.push_front(n1);
if(n1.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n2.vKeys.size()>0)
{
lNodes.push_front(n2);
if(n2.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n3.vKeys.size()>0)
{
lNodes.push_front(n3);
if(n3.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
if(n4.vKeys.size()>0)
{
lNodes.push_front(n4);
if(n4.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
lNodes.front().lit = lNodes.begin();
}
}
lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
if((int)lNodes.size()>=N)
break;
}
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
bFinish = true;
}
}
}
// Retain the best point in each node
// 保留每個區域響應值最大的一個興趣點
vector<cv::KeyPoint> vResultKeys;
vResultKeys.reserve(nfeatures);
for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
{
vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;
cv::KeyPoint* pKP = &vNodeKeys[0];
float maxResponse = pKP->response;
for(size_t k=1;k<vNodeKeys.size();k++)
{
if(vNodeKeys[k].response>maxResponse)
{
pKP = &vNodeKeys[k];
maxResponse = vNodeKeys[k].response;
}
}
vResultKeys.push_back(*pKP);
}
return vResultKeys;
}
//對影像金字塔中的每一層圖像進行特征點的計算。具體計算過程是将影像網格分割成小區域，每一個小區域獨立使用FAST角點檢測
//檢測完成之後使用DistributeOcTree函數對檢測到所有的角點進行篩選，使得角點分布均勻
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{
allKeypoints.resize(nlevels);
const float W = 30;//視窗大小
// 對每一層圖像做處理
for (int level = 0; level < nlevels; ++level) //計算邊界
{
const int minBorderX = EDGE_THRESHOLD-3; //裁邊19-3=16，
const int minBorderY = minBorderX;
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
vector<cv::KeyPoint> vToDistributeKeys;
vToDistributeKeys.reserve(nfeatures*10);
//寬度和高度
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
//寬高方向的網格數量
const int nCols = width/W;
const int nRows = height/W;
//網格的寬與高
const int wCell = ceil(width/nCols);
const int hCell = ceil(height/nRows);
//周遊每一個視窗
for(int i=0; i<nRows; i++)
{
const float iniY =minBorderY+i*hCell;
float maxY = iniY+hCell+6;//最後計算的 Cell 的寬或高不會小于7。因為 FAST 計算的鄰域是直徑為7的 BressenHam 圓。
//數字7與代碼中出現的數字6對應。
//超出區域，進行下一個循環
if(iniY>=maxBorderY-3)
continue;
//最大Y超出邊界就使用計算最寬的邊界
if(maxY>maxBorderY)
maxY = maxBorderY;
//計算每列的位置
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;
// FAST提取興趣點, 自适應門檻值
vector<cv::KeyPoint> vKeysCell;
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,iniThFAST,true);
//如果沒有找到FAST特征點，就降低門檻值重新計算
if(vKeysCell.empty())
{
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
vKeysCell,minThFAST,true);
}
//找到特征點，就将其放到vToDistributeKeys
if(!vKeysCell.empty())
{
for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
{
(*vit).pt.x+=j*wCell;
(*vit).pt.y+=i*hCell;
vToDistributeKeys.push_back(*vit);
}
}
}
}
vector<KeyPoint> & keypoints = allKeypoints[level];
keypoints.reserve(nfeatures);
// 根據mnFeaturesPerLevel,即該層的興趣點數,對特征點進行剔除，采用Harris角點的score進行排序
keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
// Add border to coordinates and scale information
const int nkps = keypoints.size();
for(int i=0; i<nkps ; i++)
{
keypoints[i].pt.x+=minBorderX;
keypoints[i].pt.y+=minBorderY;
keypoints[i].octave=level;
keypoints[i].size = scaledPatchSize;
}
}
// compute orientations
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
//計算關鍵點
void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allKeypoints)
{
allKeypoints.resize(nlevels);
float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows;//圖像縱橫比
for (int level = 0; level < nlevels; ++level)
{
const int nDesiredFeatures = mnFeaturesPerLevel[level];
const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio)); //論文中提到的每個網格5個點嗎？
const int levelRows = imageRatio*levelCols;
//得到每一層圖像進行特征檢測區域的上下兩個坐标
const int minBorderX = EDGE_THRESHOLD;
const int minBorderY = minBorderX;
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD;
//将待檢測區域劃分為格子的行列個數
const int W = maxBorderX - minBorderX;
const int H = maxBorderY - minBorderY;
const int cellW = ceil((float)W/levelCols);
const int cellH = ceil((float)H/levelRows);
const int nCells = levelRows*levelCols;
const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells);//每一個cell中特征點的個數
//Vector<T> v(n,i)，向量V中含有n個值為i 的元素 means cellKeypoint has levelRows層，每一層中又有levelCols層，均初始化為0
vector<vector<vector<KeyPoint> > > cellKeyPoints(levelRows, vector<vector<KeyPoint> >(levelCols));
vector<vector<int> > nToRetain(levelRows,vector<int>(levelCols,0));
vector<vector<int> > nTotal(levelRows,vector<int>(levelCols,0));
vector<vector<bool> > bNoMore(levelRows,vector<bool>(levelCols,false));
vector<int> iniXCol(levelCols);
vector<int> iniYRow(levelRows);
int nNoMore = 0;
int nToDistribute = 0;
float hY = cellH + 6;
for(int i=0; i<levelRows; i++)
{
const float iniY = minBorderY + i*cellH - 3;//第i個cell的第一個Y
iniYRow[i] = iniY;// vector<int> iniYRow(levelRows)
if(i == levelRows-1)//如果循環到最後一個
{
hY = maxBorderY+3-iniY;//hY=3+Ymax-iniY=3+Ymax-(Ymin+(levelRows-1)*cellH -3)=6+Ymax-Ymin-H+cellH=cellH+6
if(hY<=0) //hY牽扯到後面cellimage的大小 範圍從iniY到 iniY+hY，不可能為負值
continue; //continue 隻管for、while，不看if，不管多少if都直接無視;如果小于直接跳出本次循環，根據上一個注釋的式子，正常是不會小于的
}
float hX = cellW + 6;
for(int j=0; j<levelCols; j++)
{
float iniX;
if(i==0)
{
iniX = minBorderX + j*cellW - 3; //和上面計算的y其實是關于某條線對稱的
iniXCol[j] = iniX;
}
else
{
iniX = iniXCol[j];// 和第一行的x值對齊
}
if(j == levelCols-1)
{
hX = maxBorderX+3-iniX;
if(hX<=0)
continue;
}
Mat cellImage = mvImagePyramid[level].rowRange(iniY,iniY+hY).colRange(iniX,iniX+hX);
cellKeyPoints[i][j].reserve(nfeaturesCell*5);//論文中至少5個點
FAST(cellImage,cellKeyPoints[i][j],iniThFAST,true);//FAST檢測關鍵子
if(cellKeyPoints[i][j].size()<=3)
{
cellKeyPoints[i][j].clear();
FAST(cellImage,cellKeyPoints[i][j],minThFAST,true);//降低門檻值重新檢測關鍵子
}
const int nKeys = cellKeyPoints[i][j].size();//網格中到底有多少關鍵點
nTotal[i][j] = nKeys;
if(nKeys>nfeaturesCell) //網格中的關鍵點比需要的要多
{
nToRetain[i][j] = nfeaturesCell; //儲存預先計算好的關鍵點
bNoMore[i][j] = false;
}
else
{
nToRetain[i][j] = nKeys;
nToDistribute += nfeaturesCell-nKeys;
bNoMore[i][j] = true;
nNoMore++;
}
}
}
// Retain by score 如果 總共的離散點數大于0并且 未達到門檻值的cell數目比總共的格網數小；直到不需要離散 不需要加點為止
while(nToDistribute>0 && nNoMore<nCells)
{
int nNewFeaturesCell = nfeaturesCell + ceil((float)nToDistribute/(nCells-nNoMore)); //不夠的cell需要加入後 新的點的數目（舊的加上均分的新的）
nToDistribute = 0;
for(int i=0; i<levelRows; i++)
{
for(int j=0; j<levelCols; j++)
{
if(!bNoMore[i][j]) //有足夠點數的cell
{
if(nTotal[i][j]>nNewFeaturesCell) //總數目甚至比新的要求的點數還要多（當所有cell都執行這個條件語句，while循環就可以終止了）
{
nToRetain[i][j] = nNewFeaturesCell;//隻儲存新要求的點的數目即可
bNoMore[i][j] = false;
}
else
{
nToRetain[i][j] = nTotal[i][j];
nToDistribute += nNewFeaturesCell-nTotal[i][j];//還要離散的點的數目
bNoMore[i][j] = true; //還需要在加點
nNoMore++;
}
}
}
}
}
vector<KeyPoint> & keypoints = allKeypoints[level];
keypoints.reserve(nDesiredFeatures*2);
const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
// Retain by score and transform coordinates 換算特征點真實位置（添加邊界值），添加特征點的尺度資訊
for(int i=0; i<levelRows; i++)
{
for(int j=0; j<levelCols; j++)
{
vector<KeyPoint> &keysCell = cellKeyPoints[i][j];
KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]);//儲存最佳點
if((int)keysCell.size()>nToRetain[i][j])
keysCell.resize(nToRetain[i][j]);
for(size_t k=0, kend=keysCell.size(); k<kend; k++)
{
keysCell[k].pt.x+=iniXCol[j];
keysCell[k].pt.y+=iniYRow[i];
keysCell[k].octave=level;
keysCell[k].size = scaledPatchSize;
keypoints.push_back(keysCell[k]);
}
}
}
//特征點還多的話，在進行一次濾波
if((int)keypoints.size()>nDesiredFeatures)
{
KeyPointsFilter::retainBest(keypoints,nDesiredFeatures);
keypoints.resize(nDesiredFeatures);
}
}
// and compute orientations
for (int level = 0; level < nlevels; ++level)
computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}
//計算描述子
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
const vector<Point>& pattern)
{
descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
for (size_t i = 0; i < keypoints.size(); i++)
computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));
}
//輸入的變量
// _image:擷取的灰階圖像
// _mask:掩碼
// _keypoints:關鍵點
// _descriptors:描述子
//括号運算符輸入圖像，并且傳入引用參數_keypoints,_descriptors用于計算得到的特征點及其描述子
// 這種設計使得隻需要構造一次ORBextractor就可以為為所有圖像生成特征點
void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors)
{
if(_image.empty())
return;
Mat image = _image.getMat();
assert(image.type() == CV_8UC1 ); //若錯誤則終止程式
// Pre-compute the scale pyramid
// 建構圖像金字塔
ComputePyramid(image);
// 計算每層圖像的興趣點
vector < vector<KeyPoint> > allKeypoints; // vector<vector<KeyPoint>>
ComputeKeyPointsOctTree(allKeypoints);
//ComputeKeyPointsOld(allKeypoints);
Mat descriptors;
int nkeypoints = 0;
for (int level = 0; level < nlevels; ++level)
nkeypoints += (int)allKeypoints[level].size();
if( nkeypoints == 0 )
_descriptors.release();
else
{
_descriptors.create(nkeypoints, 32, CV_8U);
descriptors = _descriptors.getMat();
}
_keypoints.clear();
_keypoints.reserve(nkeypoints);
int offset = 0;
for (int level = 0; level < nlevels; ++level)
{
vector<KeyPoint>& keypoints = allKeypoints[level];
int nkeypointsLevel = (int)keypoints.size();
if(nkeypointsLevel==0)
continue;
// preprocess the resized image 對圖像進行高斯模糊
Mat workingMat = mvImagePyramid[level].clone();
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
// Compute the descriptors 計算描述子，采用高斯分布取點，就是上面的patten
Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
computeDescriptors(workingMat, keypoints, desc, pattern);
offset += nkeypointsLevel;
// Scale keypoint coordinates對關鍵點的位置坐做尺度恢複，恢複到原圖的位置
if (level != 0)
{
float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
keypoint->pt *= scale;
}
//在_keypoints.end()前面插入區間keypoints.begin(), keypoints.end()的所有元素
// And add the keypoints to the output
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
}
}
/**
* 建構圖像金字塔
* @param image 輸入圖像
*/
void ORBextractor::ComputePyramid(cv::Mat image)
{
for (int level = 0; level < nlevels; ++level)
{
float scale = mvInvScaleFactor[level];
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 temp(wholeSize, image.type()), masktemp;
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
// Compute the resized image
if( level != 0 )
{
resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, cv::INTER_LINEAR);
copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101+BORDER_ISOLATED);//這裡的BORDER_ISOLATED是判斷是否超出有效區，參考文章９
}
else
{
copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
}
}
} //namespace ORB_SLAM
四、與opencv中ORB對比測試
這裡先介紹ｏｐｅｎｃｖ中ORB的使用，構造函數如下：
ORB(int nfeatures = 500, float scaleFactor = 1.2f, int nlevels = 8, int edgeThreshold = 31,int firstLevel = 0, int WTA_K=2, int scoreType=ORB::HARRIS_SCORE, int patchSize=31 );
可見ｏｐｅｎｃｖ中是可以選角點評分的：scoreType=ORB::HARRIS_SCORE
下面給出簡單的測試代碼：
#include <iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "ORBextractor.h"
using namespace ORB_SLAM2;
using namespace std;
using namespace cv;
int main(int argc, char **argv)
{
VideoCapture cap;
cap.open(0);
if(!cap.isOpened())//如果視訊不能正常打開則傳回
return -1;
ORBextractor *pORBextractor= new ORBextractor(100,1.2,8,20,7);
Mat frame,im;
while(1)
{
cap>>frame;//等價于cap.read(frame);
if(frame.empty())//如果某幀為空則退出循環
break;
imshow("video", frame);
cvtColor(frame,im,CV_RGB2GRAY);
//orb-salm orb/
std::vector<cv::KeyPoint> vKeys;
cv::Mat Descriptors;
(*pORBextractor)(im,cv::Mat(),vKeys,Descriptors);
Mat outimg1;
drawKeypoints( frame, vKeys, outimg1,Scalar::all(-1),DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
//drawKeypoints( frame, vKeys, outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT );
imshow("ORB特征點",outimg1);
//opencv orb/
// ORB特征點檢測
ORB orb(100);
orb.detect(frame, vKeys);
// 繪制關鍵點
Mat keypoint_img;
drawKeypoints(frame,vKeys, keypoint_img, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
//drawKeypoints(frame,vKeys, keypoint_img, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
imshow("opencv KeyPoints Image", keypoint_img);
if( 'q'==waitKey(20)){break;};//每幀延時20毫秒
}
cap.release();//釋放資源
return 0;
}
測試結果如下：
五、總結
１．orb-slam中的特征點更豐富，因為兩次高低門檻值，使得更靈活，同時低門檻值帶來了一些不穩定的點，動态視訊流中有頻繁跳動的點。
２．opencv中特征點更為密集，這是orb-slam中４叉樹的功勞。
六、參考
- ORBSLAM2學習（一）：ORB算法原理:https://blog.csdn.net/lwx309025167/article/details/80365075
- BRIEF特征點描述子:https://www.cnblogs.com/wyuzl/p/7838103.html
- 【特征檢測】FAST特征點檢測算法:https://blog.csdn.net/hujingshuang/article/details/46898007
- 【特征檢測】Harris角點檢測中的數學推導:https://blog.csdn.net/hujingshuang/article/details/46829627
- ORBSLAM2學習（二）：ORB源碼分析:https://blog.csdn.net/lwx309025167/article/details/80421077
- ORBSLAM2 特征點提取代碼注釋:https://blog.csdn.net/u014797790/article/details/79776339
- orb:https://www.cnblogs.com/wall-e2/p/8057448.html
- ORB SLAM2學習筆記之mono_kitti（三）:https://blog.csdn.net/weixin_43527185/article/details/84029903
- OpenCV的BorderTypes:https://cloud.tencent.com/developer/article/1350371
![WiFi SLAM包配置記錄WiFi SLAM包配置記錄然後，按照前面編譯順序，依次往下編譯即可，編譯完成之後就可以使用wifislam了[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)