ORB-SLAM中 ORBextract.cpp程式碼解讀
阿新 • • 發佈:2019-02-12
轉載請註明 CSDN Min220 原文網址
歡迎學習討論交流!有誤之處指出!
using namespace cv; using namespace std; namespace ORB_SLAM { const float HARRIS_K = 0.04f; const int PATCH_SIZE = 31; const int HALF_PATCH_SIZE = 15; const int EDGE_THRESHOLD = 16; //////////////////////////////////////////////////////////////////////////////////////////////////////////// /*---------------------------------------get the HarrisResponse-------------------------------------------*/ static void HarrisResponses(const Mat& img, vector<KeyPoint>& pts, int blockSize, float harris_k) { //CV_Assert() means if the value in() is false,return a mistake message CV_Assert( img.type() == CV_8UC1 && blockSize*blockSize <= 2048 ); size_t ptidx, ptsize = pts.size(); const uchar* ptr00 = img.ptr<uchar>(); // get the ptr00 row in image int step = (int)(img.step/img.elemSize1()); // the number of bytes of a element' one channel int r = blockSize/2; // circle radius R float scale = (1 << 2) * blockSize * 255.0f; // 255.0f f means float,we can ignore,1.0f is same scale = 1.0f / scale; float scale_sq_sq = scale * scale * scale * scale; AutoBuffer<int> ofsbuf(blockSize*blockSize); int* ofs = ofsbuf; for( int i = 0; i < blockSize; i++ ) for( int j = 0; j < blockSize; j++ ) ofs[i*blockSize + j] = (int)(i*step + j); for( ptidx = 0; ptidx < ptsize; ptidx++ ) { int x0 = cvRound(pts[ptidx].pt.x - r); int y0 = cvRound(pts[ptidx].pt.y - r); const uchar* ptr0 = ptr00 + y0*step + x0; int a = 0, b = 0, c = 0; for( int k = 0; k < blockSize*blockSize; k++ ) { const uchar* ptr = ptr0 + ofs[k]; int Ix = (ptr[1] - ptr[-1])*2 + (ptr[-step+1] - ptr[-step-1]) + (ptr[step+1] - ptr[step-1]); int Iy = (ptr[step] - ptr[-step])*2 + (ptr[step-1] - ptr[-step-1]) + (ptr[step+1] - ptr[-step+1]); a += Ix*Ix; b += Iy*Iy; c += Ix*Iy; } // Harris response function pts[ptidx].response = ((float)a * b - (float)c * c - harris_k * ((float)a + b) * ((float)a + b))*scale_sq_sq; } } //////////////////////////////////////////////////////////////////////////////////////////////////////////// /*-----------------------------------get the angles of keypoints------------------------------------------*/ static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max) { int m_01 = 0, m_10 = 0; // center means a cvRound 2D coordinate in image const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x)); // Treat the center line differently, v=0 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(); 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);//這邊加是由於u已經確定好了符號 } m_01 += v * v_sum; } return fastAtan2((float)m_01, (float)m_10); } //////////////////////////////////////////////////////////////////////////////////////////////////////////// /*-----------------------------------compute the descriptors------------------------------------------*/ 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) { int t0, t1, val; t0 = GET_VALUE(0); t1 = GET_VALUE(1); val = t0 < t1; t0 = GET_VALUE(2); t1 = GET_VALUE(3); val |= (t0 < t1) << 1; t0 = GET_VALUE(4); t1 = GET_VALUE(5); val |= (t0 < t1) << 2; t0 = GET_VALUE(6); t1 = GET_VALUE(7); val |= (t0 < t1) << 3; t0 = GET_VALUE(8); t1 = GET_VALUE(9); val |= (t0 < t1) << 4; t0 = GET_VALUE(10); t1 = GET_VALUE(11); val |= (t0 < t1) << 5; t0 = GET_VALUE(12); t1 = GET_VALUE(13); val |= (t0 < t1) << 6; t0 = GET_VALUE(14); t1 = GET_VALUE(15); val |= (t0 < t1) << 7; desc[i] = (uchar)val; } #undef GET_VALUE } static int bit_pattern_31_[256*4] = { //省略 }; ////////////////////////////////////////////////////////////////////////////////////////////////////// ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels, int _scoreType, int _fastTh): nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), scoreType(_scoreType), fastTh(_fastTh) { /*------------------------- 確定每一層的特徵點數 ,使用等比數列----------------------------------*/ // 先定義尺度大小和逆尺度大小 mvScaleFactor.resize(nlevels); mvScaleFactor[0]=1; // the factor of th first level for(int i=1; i<nlevels; i++) mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor; float invScaleFactor = 1.0f/scaleFactor; mvInvScaleFactor.resize(nlevels); mvInvScaleFactor[0]=1; for(int i=1; i<nlevels; i++) mvInvScaleFactor[i]=mvInvScaleFactor[i-1]*invScaleFactor; mvImagePyramid.resize(nlevels); mvMaskPyramid.resize(nlevels); mnFeaturesPerLevel.resize(nlevels); //包含 所有 每一層所要的特徵點數目 float factor = (float)(1.0 / scaleFactor); //scaleFactor is a input float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels)); // 每層特徵點數目 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); //最大層特徵點數目=需要的特徵點數目-現有總共的特徵點數目 /*--------------------------------------------- 複製訓練的模板 ----------------------------------------------*/ // copy (IuputIterator_First, InoutIterator_Last, OutputIterator_Dest) 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 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); 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 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 ORBextractor::ComputeKeyPoints(vector<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)); //是?論文裡提到:每個格網裡面至少五個點?? 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的元素 vector<vector<vector<KeyPoint> > > cellKeyPoints(levelRows, vector<vector<KeyPoint> >(levelCols)); //means cellKeypoint has levelRows層,每一層中又有levelCols層,均初始化為0 vector<vector<int> > nToRetain(levelRows,vector<int>(levelCols)); vector<vector<int> > nTotal(levelRows,vector<int>(levelCols)); 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; // 關於3 每一行都留出3個畫素的寬度 // 在每個格子內進行fast特徵檢測 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); Mat cellMask; if(!mvMaskPyramid[level].empty()) cellMask = cv::Mat(mvMaskPyramid[level],Rect(iniX,iniY,hX,hY)); //rect物件用來儲存一個矩形框的左上角座標、寬度和高度。 cellKeyPoints[i][j].reserve(nfeaturesCell*5); //論文中的至少5個點 /*---------------------------------FAST檢測-,還在迴圈中---------------------------------*/ FAST(cellImage,cellKeyPoints[i][j],fastTh,true); //FAST函式檢測關鍵子 /*CV_EXPORTS void FAST( InputArray image, CV_OUT vector<KeyPoint>& keypoints, int threshold, bool nonmaxSuppression=true );*/ if(cellKeyPoints[i][j].size()<=3) { cellKeyPoints[i][j].clear(); FAST(cellImage,cellKeyPoints[i][j],7,true); // 閾值為7 } if( scoreType == ORB::HARRIS_SCORE ) { // Compute the Harris corner HarrisResponses(cellImage,cellKeyPoints[i][j], 7, HARRIS_K); } const int nKeys = cellKeyPoints[i][j].size(); //格網裡到底有多少個關鍵點 nTotal[i][j] = nKeys; //nTotal總點數 if(nKeys>nfeaturesCell) //如果格網裡的點數比打算的要多 { nToRetain[i][j] = nfeaturesCell; //儲存預先計算好的數目的點 bNoMore[i][j] = false; //nomore為假 } else { nToRetain[i][j] = nKeys; //否則先知道要儲存的數目 nToDistribute += nfeaturesCell-nKeys; //還有多少需要離散的點的數目 bNoMore[i][j] = true; nNoMore++; } } } // Retain by score while(nToDistribute>0 && nNoMore<nCells) //如果 總共的離散點數大於0並且 未達到閾值的cell數目比總共的格網數小;直到不需要離散 不需要加點為止 { 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++; } } } } }
//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); Mat cellMask; if(!mvMaskPyramid[level].empty()) cellMask = cv::Mat(mvMaskPyramid[level],Rect(iniX,iniY,hX,hY)); //rect物件用來儲存一個矩形框的左上角座標、寬度和高度。 cellKeyPoints[i][j].reserve(nfeaturesCell*5); //論文中的至少5個點 /*---------------------------------FAST檢測-,還在迴圈中---------------------------------*/ FAST(cellImage,cellKeyPoints[i][j],fastTh,true); //FAST函式檢測關鍵子 /*CV_EXPORTS void FAST( InputArray image, CV_OUT vector<KeyPoint>& keypoints, int threshold, bool nonmaxSuppression=true );*/ if(cellKeyPoints[i][j].size()<=3) { cellKeyPoints[i][j].clear(); FAST(cellImage,cellKeyPoints[i][j],7,true); // 閾值為7 } if( scoreType == ORB::HARRIS_SCORE ) { // Compute the Harris corner HarrisResponses(cellImage,cellKeyPoints[i][j], 7, HARRIS_K); } const int nKeys = cellKeyPoints[i][j].size(); //格網裡到底有多少個關鍵點 nTotal[i][j] = nKeys; //nTotal總點數 if(nKeys>nfeaturesCell) //如果格網裡的點數比打算的要多 { nToRetain[i][j] = nfeaturesCell; //儲存預先計算好的數目的點 bNoMore[i][j] = false; //nomore為假 } else { nToRetain[i][j] = nKeys; //否則先知道要儲存的數目 nToDistribute += nfeaturesCell-nKeys; //還有多少需要離散的點的數目 bNoMore[i][j] = true; nNoMore++; } } } // Retain by score while(nToDistribute>0 && nNoMore<nCells) //如果 總共的離散點數大於0並且 未達到閾值的cell數目比總共的格網數小;直到不需要離散 不需要加點為止 { 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));
}
/*--------------------------------------到了operator啦------------------------------------------*/
void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors)
{
if(_image.empty())
return;
Mat image = _image.getMat(), mask = _mask.getMat();
assert(image.type() == CV_8UC1 );
// Pre-compute the scale pyramids
ComputePyramid(image, mask);
vector < vector<KeyPoint> > allKeypoints;
ComputeKeyPoints(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];
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
// Compute the descriptors
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;
}
// And add the keypoints to the output
_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
}
}
/*-------------------------------------------金字塔建立-----------------------------------------*/
void ORBextractor::ComputePyramid(cv::Mat image, cv::Mat Mask)
{
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));
if( !Mask.empty() )
{
masktemp = Mat(wholeSize, Mask.type());
mvMaskPyramid[level] = masktemp(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, INTER_LINEAR);
if (!Mask.empty())
{
resize(mvMaskPyramid[level-1], mvMaskPyramid[level], sz, 0, 0, INTER_NEAREST);
}
copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101+BORDER_ISOLATED);
if (!Mask.empty())
copyMakeBorder(mvMaskPyramid[level], masktemp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_CONSTANT+BORDER_ISOLATED);
}
else
{
copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
if( !Mask.empty() )
copyMakeBorder(Mask, masktemp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_CONSTANT+BORDER_ISOLATED);
}
}
}
} //namespace ORB_SLAM