1. 理论知识特征点由关键点 (Key-point) 和描述子 (Descriptor) 两部分组成。ORB特征点 (Oriented FAST and Rotated BRIEF) 是由Oriented FAST角点和 BRIEF (Binary Robust Independent Elementary Features) 描述子构成其计算速度是sift特征点的100倍是surf特征点的10倍。1.1. Fast 角点提取FAST 是一种角点主要检测局部像素灰度变化明显的地方以速度快著称。它的思想是如果一个像素与它邻域的像素差别较大过亮或过暗那它更可能是角点。相比于其他角点检测算法FAST 只需比较像素亮度的大小十分快捷。提取步骤在图像中选取像素 p假设它的亮度为 Ip。设置一个阈值 T比如 Ip 的 20%。以像素 p 为中心选取半径为 3 的圆上的 16 个像素点。假如选取的圆上有连续的 N 个点的亮度大于 Ip T 或小于 Ip −T那么像素 p 可以被认为是特征点N 通常取 12即为 FAST-12。其它常用的 N 取值为 9 和 11他们分别被称为 FAST-9FAST-11。循环以上四步对每一个像素执行相同的操作。为了提高效率可以采用额外的加速办法。具体操作为对于每个像素直接检测邻域圆上的第 15913 个像素的亮度。至少有3个和候选点的灰度值同时大于 Ip T 或小于 Ip −T 时当前像素才有可能是一个角点否则应该直接排除。为了提高比较的效率通常只使用N个周边像素来比较也就是大家经常说的FAST-N其中Fast-9Fast-12使用最多。Fast角点本不具有方向但是由于特征点匹配需要ORB对Fast角点进行了改进改进后的 FAST 被称为 Oriented FAST具有旋转和尺度的描述。尺度不变性是由构建图像金字塔并在金字塔的每一层上检测角点来实现这在SLAM十四讲中并没有具体介绍。这里进行介绍。1.2. 高斯金字塔构建对图像做不同尺度的高斯模糊为了让尺度体现其连续性高斯金字塔在简单降采样的基础上加上了高斯滤波。将图像金字塔每层的一张图像使用不同参数做高斯模糊使得金字塔的每层含有多张高斯模糊图像将金字塔每层多张图像合称为一组 (Octave)金字塔每层只有一组图像组数和金字塔层数相等使用下列公式计算每组含有多张也叫层Interval图像。另外降采样时高斯金字塔上一组图像的初始图像底层图像是由前一组图像的倒数第三张图像隔点采样得到的。其中MN为原图像的大小t为塔顶图像的最小维数的对数值。如对于大小为512*512的图像金字塔上各层图像的大小如表3.1所示当塔顶图像为4*4时n7当塔顶图像为2*2时n8。对图像做降采样隔点采样1.3. 总结设置一个比例因子scaleFactorOpenCV默认为1.2和金字塔的层数nlevelsOpenCV默认为8。将原图像按比例因子缩小成nlevels幅图像。缩放后的图像为I’ I/scaleFactork (k1, 2, …, nlevels)。nlevels幅不同比例的图像提取特征点总和作为这幅图像的oFAST特征点。特征的旋转是由灰度质心法 (Intensity Centroid) 实现的。下面介绍灰度质心法。1.4. 灰度质心法在一个小的图像块 B 中定义图像块的矩为式中p, q 取0或者1I(x, y) 表示在像素坐标 (x, y) 处图像的灰度值表示图像的矩。在半径为R的圆形图像区域沿两个坐标轴x, y方向的图像矩分别为圆形区域内所有像素的灰度值总和为通过矩可以找到图像块的质心连接图像块的几何中心 O 与质心 C得到一个方向向量于是特征点的方向可以定义为2. ORB特征匹配我们将Fast角点提取出来后要描述它。否则我们无法进行匹配。ORB采用BRIEF算法来计算一个特征点的描述子。其核心思想是在关键点P的周围以一定模式选取N个点对把这N个点对的比较结果组合起来作为描述子。步骤以关键点P为圆心以d为半径做圆O。在圆O内某一模式选取N个点对。这里为方便说明N4实际应用中N可以取512.假设当前选取的4个点对如上图所示分别标记为定义操作T分别对已选取的点对进行T操作将得到的结果进行组合。假如则最终的描述子为1011ORB特征点匹配用的是汉明距离两个等长字符串之间的汉明距离是两个字符串对应位置的不同字符的个数。换句话说它就是将一个字符串变换成另外一个字符串所需要替换的字符个数。例如1011101 与 1001001 之间的汉明距离是 2。当两个特征点的汉明距离小于设定的阈值时可以认为匹配。与ORB特征的提取和匹配相比SIFT要复杂的多但是SIFT效果要更好。3. 代码实现ORB-SLAM2中提取ORB特征是由ORBextractor.cc实现的。我们先来看看主要函数。static float IC_Angle(const Mat image, Point2f pt, const vectorint u_max) { int m_01 0, m_10 0; const uchar* center image.atuchar (cvRound(pt.y), cvRound(pt.x)); // Treat the center line differently, v0 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); } 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.atuchar(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 } ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels, int _iniThFAST, int _minThFAST): nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), iniThFAST(_iniThFAST), minThFAST(_minThFAST) { // nfeatures期望提取的特征点个数 // nlevels金字塔层数 // scaleFactor相邻两层金字塔之间的相对尺度因子大于1金字塔越往上的图像每个像素代表的范围越大 // mvScaleFactor累乘得到每一层相对第一层的尺度因子 // mvLevelSigma2尺度因子mvScaleFactor的平方 // mvInvScaleFactor尺度因子mvScaleFactor的逆 // mvInvLevelSigma2尺度因子平方mvLevelSigma2的逆 // mnFeaturesPerLevel记录每一层期望提取的特征点个数 // iniThFAST提取fast特征点的默认阈值 // minThFAST如果使用iniThFAST默认阈值提取不到特征点则使用最小阈值再次提取 mvScaleFactor.resize(nlevels); mvLevelSigma2.resize(nlevels); mvScaleFactor[0]1.0f; mvLevelSigma2[0]1.0f; for(int i1; inlevels; i) { mvScaleFactor[i]mvScaleFactor[i-1]*scaleFactor; mvLevelSigma2[i]mvScaleFactor[i]*mvScaleFactor[i]; } mvInvScaleFactor.resize(nlevels); mvInvLevelSigma2.resize(nlevels); for(int i0; inlevels; i) { mvInvScaleFactor[i]1.0f/mvScaleFactor[i]; mvInvLevelSigma2[i]1.0f/mvLevelSigma2[i]; } mvImagePyramid.resize(nlevels); mnFeaturesPerLevel.resize(nlevels); float factor 1.0f / scaleFactor; // 总共期望提取nfeatures个特征点根据尺度因子等比数列计算出金字塔最底层期望提取的特征点个数 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); 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 umax.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 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, vectorKeyPoint keypoints, const vectorint umax) { for (vectorKeyPoint::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_castfloat(UR.x-UL.x)/2); const int halfY ceil(static_castfloat(BR.y-UL.y)/2); //Define boundaries of childs n1.UL UL; n1.UR cv::Point2i(UL.xhalfX,UL.y); n1.BL cv::Point2i(UL.x,UL.yhalfY); n1.BR cv::Point2i(UL.xhalfX,UL.yhalfY); n1.vKeys.reserve(vKeys.size()); n2.UL n1.UR; n2.UR UR; n2.BL n1.BR; n2.BR cv::Point2i(UR.x,UL.yhalfY); 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 i0;ivKeys.size();i) { const cv::KeyPoint kp vKeys[i]; if(kp.pt.xn1.UR.x) { if(kp.pt.yn1.BR.y) n1.vKeys.push_back(kp); else n3.vKeys.push_back(kp); } else if(kp.pt.yn1.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; } vectorcv::KeyPoint ORBextractor::DistributeOctTree(const vectorcv::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_castfloat(maxX-minX)/(maxY-minY)); // note如果图像的宽不到高的一半hX0会出问题据此推断这里默认为图像宽大于高的情况 const float hX static_castfloat(maxX-minX)/nIni; // lNodes用于存放节点数据note只保留叶子节点 // ExtractorNode中UL、UR、BL、BR记录了该节点区域的四个顶点坐标 // ExtractorNode中的vKeys记录了属于该节点区域的所有特征点这里有些低效容器里存的是特征点而不是特征点的指针 listExtractorNode lNodes; // 记录初始节点的指针为了方便根据特征点x坐标快速找到对应的节点x/hX vectorExtractorNode* vpIniNodes; vpIniNodes.resize(nIni); // step1: 建立分裂的初始节点 // step1.1确定节点区域 for(int i0; inIni; i) { ExtractorNode ni; ni.UL cv::Point2i(hX*static_castfloat(i),0); ni.UR cv::Point2i(hX*static_castfloat(i1),0); ni.BL cv::Point2i(ni.UL.x,maxY-minY); // wubo为什么要减去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 // step1.2将所有特征点关联到对应的节点区域 for(size_t i0;ivToDistributeKeys.size();i) { const cv::KeyPoint kp vToDistributeKeys[i]; vpIniNodes[kp.pt.x/hX]-vKeys.push_back(kp); } listExtractorNode::iterator lit lNodes.begin(); while(lit!lNodes.end()) { if(lit-vKeys.size()1) // 如果这个区域只有一个特征点则不用再构建子树 { lit-bNoMoretrue; lit; } else if(lit-vKeys.empty()) // 如果这个区域一个特征点都没有则删除该空节点 lit lNodes.erase(lit); else lit; } bool bFinish false; int iteration 0; vectorpairint,ExtractorNode* vSizeAndPointerToNode; vSizeAndPointerToNode.reserve(lNodes.size()*4); // 利用四叉树方法对图像进行划分区域 while(!bFinish) { iteration; int prevSize lNodes.size(); lit lNodes.begin(); int nToExpand 0; vSizeAndPointerToNode.clear(); // step2广度搜索的方式遍历所有节点将目前的子区域进行划分 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) { // note将新分裂出的节点插入到容器前面迭代器后面的都是上一次分裂还未访问的节点 lNodes.push_front(n1); // 如果该节点中包含的特征点超过1则该节点将会继续扩展子节点使用nToExpand统计接下来要扩展的节点数 if(n1.vKeys.size()1) { nToExpand; // 按照 pair节点中特征点个数节点索引 建立索引后续通过排序快速筛选出包含特征点个数比较多的节点 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(); } } // 该节点已经分裂完删除该节点 litlNodes.erase(lit); continue; } } // step3Node数快接近要求数目时优先对包含特征点比较多的区域进行划分 // 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; } else if(((int)lNodes.size()nToExpand*3)N) // 当再划分之后所有的Node数快接近要求数目时优先对包含特征点比较多的区域进行划分 { while(!bFinish) { prevSize lNodes.size(); vectorpairint,ExtractorNode* vPrevSizeAndPointerToNode vSizeAndPointerToNode; vSizeAndPointerToNode.clear(); // 对需要划分的部分进行排序, 即对兴趣点数较多的区域进行划分 sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end()); for(int jvPrevSizeAndPointerToNode.size()-1;j0;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 // step4保留每个区域响应值最大的一个兴趣点 vectorcv::KeyPoint vResultKeys; vResultKeys.reserve(nfeatures); for(listExtractorNode::iterator litlNodes.begin(); lit!lNodes.end(); lit) { vectorcv::KeyPoint vNodeKeys lit-vKeys; cv::KeyPoint* pKP vNodeKeys[0]; float maxResponse pKP-response; for(size_t k1;kvNodeKeys.size();k) { if(vNodeKeys[k].responsemaxResponse) { pKP vNodeKeys[k]; maxResponse vNodeKeys[k].response; } } vResultKeys.push_back(*pKP); } return vResultKeys; } void ORBextractor::ComputeKeyPointsOctTree(vectorvectorKeyPoint allKeypoints) { allKeypoints.resize(nlevels); const float W 30; // 对金字塔每一层图像提取特征点 for (int level 0; level nlevels; level) { const int minBorderX EDGE_THRESHOLD-3; const int minBorderY minBorderX; const int maxBorderX mvImagePyramid[level].cols-EDGE_THRESHOLD3; const int maxBorderY mvImagePyramid[level].rows-EDGE_THRESHOLD3; vectorcv::KeyPoint vToDistributeKeys; vToDistributeKeys.reserve(nfeatures*10); const float width (maxBorderX-minBorderX); const float height (maxBorderY-minBorderY); // 每个区域块的大小为W将图像划分为nRows*nCols个区域在无法取整的情况下调整每个区域大小为wCell*hCell const int nCols width/W; const int nRows height/W; const int wCell ceil(width/nCols); const int hCell ceil(height/nRows); // wubo 如果直接对整张图进行特征点检测则对检测结果判断每个区域内是否有特征点会比较麻烦因此这里按照一个区域一个区域的方式检测特征点 // 按区域提取特征点--- vToDistributeKeys for(int i0; inRows; i) { // 计算每个块的Y方向上起始和终止区域iniYmaxY const float iniY minBorderYi*hCell; float maxY iniYhCell6; if(iniYmaxBorderY-3) continue; if(maxYmaxBorderY) maxY maxBorderY; for(int j0; jnCols; j) { // 计算每个块的X方向上起始和终止区域iniXmaxX const float iniX minBorderXj*wCell; float maxX iniXwCell6; if(iniXmaxBorderX-6) continue; if(maxXmaxBorderX) maxX maxBorderX; // opencv/modules/features2d/src/fast.cpp // 在iniX, iniY(maxX, maxY)范围内提取FAST关键点, 并开启非极大值抑制防止在一个很小的区域内提取过多的特征点 vectorcv::KeyPoint vKeysCell; FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,iniThFAST,true); // 如果使用iniThFAST默认阈值提取不到特征点则使用最小阈值minThFAST再次提取 if(vKeysCell.empty()) { FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,minThFAST,true); } if(!vKeysCell.empty()) { for(vectorcv::KeyPoint::iterator vitvKeysCell.begin(); vit!vKeysCell.end();vit) { (*vit).pt.xj*wCell; (*vit).pt.yi*hCell; vToDistributeKeys.push_back(*vit); } } } } vectorKeyPoint keypoints allKeypoints[level]; keypoints.reserve(nfeatures); // 根据mnFeaturesPerLevel即该层的兴趣点数,对特征点进行剔除 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 i0; inkps ; i) { keypoints[i].pt.xminBorderX; keypoints[i].pt.yminBorderY; keypoints[i].octavelevel; keypoints[i].size scaledPatchSize; } } // compute orientations for (int level 0; level nlevels; level) computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); } void ORBextractor::ComputeKeyPointsOld(std::vectorstd::vectorKeyPoint 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); vectorvectorvectorKeyPoint cellKeyPoints(levelRows, vectorvectorKeyPoint (levelCols)); vectorvectorint nToRetain(levelRows,vectorint(levelCols,0)); vectorvectorint nTotal(levelRows,vectorint(levelCols,0)); vectorvectorbool bNoMore(levelRows,vectorbool(levelCols,false)); vectorint iniXCol(levelCols); vectorint iniYRow(levelRows); int nNoMore 0; int nToDistribute 0; float hY cellH 6; for(int i0; ilevelRows; i) { const float iniY minBorderY i*cellH - 3; iniYRow[i] iniY; if(i levelRows-1) { hY maxBorderY3-iniY; if(hY0) continue; } float hX cellW 6; for(int j0; jlevelCols; j) { float iniX; if(i0) { iniX minBorderX j*cellW - 3; iniXCol[j] iniX; } else { iniX iniXCol[j]; } if(j levelCols-1) { hX maxBorderX3-iniX; if(hX0) continue; } Mat cellImage mvImagePyramid[level].rowRange(iniY,iniYhY).colRange(iniX,iniXhX); cellKeyPoints[i][j].reserve(nfeaturesCell*5); FAST(cellImage,cellKeyPoints[i][j],iniThFAST,true); 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(nKeysnfeaturesCell) { nToRetain[i][j] nfeaturesCell; bNoMore[i][j] false; } else { nToRetain[i][j] nKeys; nToDistribute nfeaturesCell-nKeys; bNoMore[i][j] true; nNoMore; } } } // Retain by score while(nToDistribute0 nNoMorenCells) { int nNewFeaturesCell nfeaturesCell ceil((float)nToDistribute/(nCells-nNoMore)); nToDistribute 0; for(int i0; ilevelRows; i) { for(int j0; jlevelCols; j) { if(!bNoMore[i][j]) { if(nTotal[i][j]nNewFeaturesCell) { 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; } } } } } vectorKeyPoint keypoints allKeypoints[level]; keypoints.reserve(nDesiredFeatures*2); const int scaledPatchSize PATCH_SIZE*mvScaleFactor[level]; // Retain by score and transform coordinates for(int i0; ilevelRows; i) { for(int j0; jlevelCols; j) { vectorKeyPoint 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 k0, kendkeysCell.size(); kkend; k) { keysCell[k].pt.xiniXCol[j]; keysCell[k].pt.yiniYRow[i]; keysCell[k].octavelevel; 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, vectorKeyPoint keypoints, Mat descriptors, const vectorPoint 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)); } void ORBextractor::operator()( InputArray _image, InputArray _mask, vectorKeyPoint _keypoints, OutputArray _descriptors) { if(_image.empty()) return; Mat image _image.getMat(); assert(image.type() CV_8UC1 ); // Pre-compute the scale pyramid // 构建图像金字塔并包含边界EDGE_THRESHOLD ComputePyramid(image); // 计算每层图像的兴趣点 vector vectorKeyPoint allKeypoints; // vectorvectorKeyPoint 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) { vectorKeyPoint keypoints allKeypoints[level]; int nkeypointsLevel (int)keypoints.size(); if(nkeypointsLevel0) continue; // preprocess the resized image 对图像进行高斯模糊 Mat workingMat mvImagePyramid[level].clone(); 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 (vectorKeyPoint::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()); } } /** * 构建图像金字塔并包含边界EDGE_THRESHOLD * 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_101BORDER_ISOLATED); } else { copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, BORDER_REFLECT_101); } } }参考文献ORB-SLAM2从理论到代码实现三ORB特征提取和匹配理论和代码详解_波波菠菜的博客-CSDN博客ORB-SLAM2系列第二章——ORB 特征点提取_running snail szj的博客-CSDN博客_orb特征提取
ORB-SLAM2 从理论到代码实现（三）：ORB 特征提取和匹配理论和代码详解
1. 理论知识特征点由关键点 (Key-point) 和描述子 (Descriptor) 两部分组成。ORB特征点 (Oriented FAST and Rotated BRIEF) 是由Oriented FAST角点和 BRIEF (Binary Robust Independent Elementary Features) 描述子构成其计算速度是sift特征点的100倍是surf特征点的10倍。1.1. Fast 角点提取FAST 是一种角点主要检测局部像素灰度变化明显的地方以速度快著称。它的思想是如果一个像素与它邻域的像素差别较大过亮或过暗那它更可能是角点。相比于其他角点检测算法FAST 只需比较像素亮度的大小十分快捷。提取步骤在图像中选取像素 p假设它的亮度为 Ip。设置一个阈值 T比如 Ip 的 20%。以像素 p 为中心选取半径为 3 的圆上的 16 个像素点。假如选取的圆上有连续的 N 个点的亮度大于 Ip T 或小于 Ip −T那么像素 p 可以被认为是特征点N 通常取 12即为 FAST-12。其它常用的 N 取值为 9 和 11他们分别被称为 FAST-9FAST-11。循环以上四步对每一个像素执行相同的操作。为了提高效率可以采用额外的加速办法。具体操作为对于每个像素直接检测邻域圆上的第 15913 个像素的亮度。至少有3个和候选点的灰度值同时大于 Ip T 或小于 Ip −T 时当前像素才有可能是一个角点否则应该直接排除。为了提高比较的效率通常只使用N个周边像素来比较也就是大家经常说的FAST-N其中Fast-9Fast-12使用最多。Fast角点本不具有方向但是由于特征点匹配需要ORB对Fast角点进行了改进改进后的 FAST 被称为 Oriented FAST具有旋转和尺度的描述。尺度不变性是由构建图像金字塔并在金字塔的每一层上检测角点来实现这在SLAM十四讲中并没有具体介绍。这里进行介绍。1.2. 高斯金字塔构建对图像做不同尺度的高斯模糊为了让尺度体现其连续性高斯金字塔在简单降采样的基础上加上了高斯滤波。将图像金字塔每层的一张图像使用不同参数做高斯模糊使得金字塔的每层含有多张高斯模糊图像将金字塔每层多张图像合称为一组 (Octave)金字塔每层只有一组图像组数和金字塔层数相等使用下列公式计算每组含有多张也叫层Interval图像。另外降采样时高斯金字塔上一组图像的初始图像底层图像是由前一组图像的倒数第三张图像隔点采样得到的。其中MN为原图像的大小t为塔顶图像的最小维数的对数值。如对于大小为512*512的图像金字塔上各层图像的大小如表3.1所示当塔顶图像为4*4时n7当塔顶图像为2*2时n8。对图像做降采样隔点采样1.3. 总结设置一个比例因子scaleFactorOpenCV默认为1.2和金字塔的层数nlevelsOpenCV默认为8。将原图像按比例因子缩小成nlevels幅图像。缩放后的图像为I’ I/scaleFactork (k1, 2, …, nlevels)。nlevels幅不同比例的图像提取特征点总和作为这幅图像的oFAST特征点。特征的旋转是由灰度质心法 (Intensity Centroid) 实现的。下面介绍灰度质心法。1.4. 灰度质心法在一个小的图像块 B 中定义图像块的矩为式中p, q 取0或者1I(x, y) 表示在像素坐标 (x, y) 处图像的灰度值表示图像的矩。在半径为R的圆形图像区域沿两个坐标轴x, y方向的图像矩分别为圆形区域内所有像素的灰度值总和为通过矩可以找到图像块的质心连接图像块的几何中心 O 与质心 C得到一个方向向量于是特征点的方向可以定义为2. ORB特征匹配我们将Fast角点提取出来后要描述它。否则我们无法进行匹配。ORB采用BRIEF算法来计算一个特征点的描述子。其核心思想是在关键点P的周围以一定模式选取N个点对把这N个点对的比较结果组合起来作为描述子。步骤以关键点P为圆心以d为半径做圆O。在圆O内某一模式选取N个点对。这里为方便说明N4实际应用中N可以取512.假设当前选取的4个点对如上图所示分别标记为定义操作T分别对已选取的点对进行T操作将得到的结果进行组合。假如则最终的描述子为1011ORB特征点匹配用的是汉明距离两个等长字符串之间的汉明距离是两个字符串对应位置的不同字符的个数。换句话说它就是将一个字符串变换成另外一个字符串所需要替换的字符个数。例如1011101 与 1001001 之间的汉明距离是 2。当两个特征点的汉明距离小于设定的阈值时可以认为匹配。与ORB特征的提取和匹配相比SIFT要复杂的多但是SIFT效果要更好。3. 代码实现ORB-SLAM2中提取ORB特征是由ORBextractor.cc实现的。我们先来看看主要函数。static float IC_Angle(const Mat image, Point2f pt, const vectorint u_max) { int m_01 0, m_10 0; const uchar* center image.atuchar (cvRound(pt.y), cvRound(pt.x)); // Treat the center line differently, v0 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); } 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.atuchar(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 } ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels, int _iniThFAST, int _minThFAST): nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), iniThFAST(_iniThFAST), minThFAST(_minThFAST) { // nfeatures期望提取的特征点个数 // nlevels金字塔层数 // scaleFactor相邻两层金字塔之间的相对尺度因子大于1金字塔越往上的图像每个像素代表的范围越大 // mvScaleFactor累乘得到每一层相对第一层的尺度因子 // mvLevelSigma2尺度因子mvScaleFactor的平方 // mvInvScaleFactor尺度因子mvScaleFactor的逆 // mvInvLevelSigma2尺度因子平方mvLevelSigma2的逆 // mnFeaturesPerLevel记录每一层期望提取的特征点个数 // iniThFAST提取fast特征点的默认阈值 // minThFAST如果使用iniThFAST默认阈值提取不到特征点则使用最小阈值再次提取 mvScaleFactor.resize(nlevels); mvLevelSigma2.resize(nlevels); mvScaleFactor[0]1.0f; mvLevelSigma2[0]1.0f; for(int i1; inlevels; i) { mvScaleFactor[i]mvScaleFactor[i-1]*scaleFactor; mvLevelSigma2[i]mvScaleFactor[i]*mvScaleFactor[i]; } mvInvScaleFactor.resize(nlevels); mvInvLevelSigma2.resize(nlevels); for(int i0; inlevels; i) { mvInvScaleFactor[i]1.0f/mvScaleFactor[i]; mvInvLevelSigma2[i]1.0f/mvLevelSigma2[i]; } mvImagePyramid.resize(nlevels); mnFeaturesPerLevel.resize(nlevels); float factor 1.0f / scaleFactor; // 总共期望提取nfeatures个特征点根据尺度因子等比数列计算出金字塔最底层期望提取的特征点个数 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); 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 umax.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 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, vectorKeyPoint keypoints, const vectorint umax) { for (vectorKeyPoint::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_castfloat(UR.x-UL.x)/2); const int halfY ceil(static_castfloat(BR.y-UL.y)/2); //Define boundaries of childs n1.UL UL; n1.UR cv::Point2i(UL.xhalfX,UL.y); n1.BL cv::Point2i(UL.x,UL.yhalfY); n1.BR cv::Point2i(UL.xhalfX,UL.yhalfY); n1.vKeys.reserve(vKeys.size()); n2.UL n1.UR; n2.UR UR; n2.BL n1.BR; n2.BR cv::Point2i(UR.x,UL.yhalfY); 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 i0;ivKeys.size();i) { const cv::KeyPoint kp vKeys[i]; if(kp.pt.xn1.UR.x) { if(kp.pt.yn1.BR.y) n1.vKeys.push_back(kp); else n3.vKeys.push_back(kp); } else if(kp.pt.yn1.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; } vectorcv::KeyPoint ORBextractor::DistributeOctTree(const vectorcv::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_castfloat(maxX-minX)/(maxY-minY)); // note如果图像的宽不到高的一半hX0会出问题据此推断这里默认为图像宽大于高的情况 const float hX static_castfloat(maxX-minX)/nIni; // lNodes用于存放节点数据note只保留叶子节点 // ExtractorNode中UL、UR、BL、BR记录了该节点区域的四个顶点坐标 // ExtractorNode中的vKeys记录了属于该节点区域的所有特征点这里有些低效容器里存的是特征点而不是特征点的指针 listExtractorNode lNodes; // 记录初始节点的指针为了方便根据特征点x坐标快速找到对应的节点x/hX vectorExtractorNode* vpIniNodes; vpIniNodes.resize(nIni); // step1: 建立分裂的初始节点 // step1.1确定节点区域 for(int i0; inIni; i) { ExtractorNode ni; ni.UL cv::Point2i(hX*static_castfloat(i),0); ni.UR cv::Point2i(hX*static_castfloat(i1),0); ni.BL cv::Point2i(ni.UL.x,maxY-minY); // wubo为什么要减去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 // step1.2将所有特征点关联到对应的节点区域 for(size_t i0;ivToDistributeKeys.size();i) { const cv::KeyPoint kp vToDistributeKeys[i]; vpIniNodes[kp.pt.x/hX]-vKeys.push_back(kp); } listExtractorNode::iterator lit lNodes.begin(); while(lit!lNodes.end()) { if(lit-vKeys.size()1) // 如果这个区域只有一个特征点则不用再构建子树 { lit-bNoMoretrue; lit; } else if(lit-vKeys.empty()) // 如果这个区域一个特征点都没有则删除该空节点 lit lNodes.erase(lit); else lit; } bool bFinish false; int iteration 0; vectorpairint,ExtractorNode* vSizeAndPointerToNode; vSizeAndPointerToNode.reserve(lNodes.size()*4); // 利用四叉树方法对图像进行划分区域 while(!bFinish) { iteration; int prevSize lNodes.size(); lit lNodes.begin(); int nToExpand 0; vSizeAndPointerToNode.clear(); // step2广度搜索的方式遍历所有节点将目前的子区域进行划分 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) { // note将新分裂出的节点插入到容器前面迭代器后面的都是上一次分裂还未访问的节点 lNodes.push_front(n1); // 如果该节点中包含的特征点超过1则该节点将会继续扩展子节点使用nToExpand统计接下来要扩展的节点数 if(n1.vKeys.size()1) { nToExpand; // 按照 pair节点中特征点个数节点索引 建立索引后续通过排序快速筛选出包含特征点个数比较多的节点 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(); } } // 该节点已经分裂完删除该节点 litlNodes.erase(lit); continue; } } // step3Node数快接近要求数目时优先对包含特征点比较多的区域进行划分 // 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; } else if(((int)lNodes.size()nToExpand*3)N) // 当再划分之后所有的Node数快接近要求数目时优先对包含特征点比较多的区域进行划分 { while(!bFinish) { prevSize lNodes.size(); vectorpairint,ExtractorNode* vPrevSizeAndPointerToNode vSizeAndPointerToNode; vSizeAndPointerToNode.clear(); // 对需要划分的部分进行排序, 即对兴趣点数较多的区域进行划分 sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end()); for(int jvPrevSizeAndPointerToNode.size()-1;j0;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 // step4保留每个区域响应值最大的一个兴趣点 vectorcv::KeyPoint vResultKeys; vResultKeys.reserve(nfeatures); for(listExtractorNode::iterator litlNodes.begin(); lit!lNodes.end(); lit) { vectorcv::KeyPoint vNodeKeys lit-vKeys; cv::KeyPoint* pKP vNodeKeys[0]; float maxResponse pKP-response; for(size_t k1;kvNodeKeys.size();k) { if(vNodeKeys[k].responsemaxResponse) { pKP vNodeKeys[k]; maxResponse vNodeKeys[k].response; } } vResultKeys.push_back(*pKP); } return vResultKeys; } void ORBextractor::ComputeKeyPointsOctTree(vectorvectorKeyPoint allKeypoints) { allKeypoints.resize(nlevels); const float W 30; // 对金字塔每一层图像提取特征点 for (int level 0; level nlevels; level) { const int minBorderX EDGE_THRESHOLD-3; const int minBorderY minBorderX; const int maxBorderX mvImagePyramid[level].cols-EDGE_THRESHOLD3; const int maxBorderY mvImagePyramid[level].rows-EDGE_THRESHOLD3; vectorcv::KeyPoint vToDistributeKeys; vToDistributeKeys.reserve(nfeatures*10); const float width (maxBorderX-minBorderX); const float height (maxBorderY-minBorderY); // 每个区域块的大小为W将图像划分为nRows*nCols个区域在无法取整的情况下调整每个区域大小为wCell*hCell const int nCols width/W; const int nRows height/W; const int wCell ceil(width/nCols); const int hCell ceil(height/nRows); // wubo 如果直接对整张图进行特征点检测则对检测结果判断每个区域内是否有特征点会比较麻烦因此这里按照一个区域一个区域的方式检测特征点 // 按区域提取特征点--- vToDistributeKeys for(int i0; inRows; i) { // 计算每个块的Y方向上起始和终止区域iniYmaxY const float iniY minBorderYi*hCell; float maxY iniYhCell6; if(iniYmaxBorderY-3) continue; if(maxYmaxBorderY) maxY maxBorderY; for(int j0; jnCols; j) { // 计算每个块的X方向上起始和终止区域iniXmaxX const float iniX minBorderXj*wCell; float maxX iniXwCell6; if(iniXmaxBorderX-6) continue; if(maxXmaxBorderX) maxX maxBorderX; // opencv/modules/features2d/src/fast.cpp // 在iniX, iniY(maxX, maxY)范围内提取FAST关键点, 并开启非极大值抑制防止在一个很小的区域内提取过多的特征点 vectorcv::KeyPoint vKeysCell; FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,iniThFAST,true); // 如果使用iniThFAST默认阈值提取不到特征点则使用最小阈值minThFAST再次提取 if(vKeysCell.empty()) { FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,minThFAST,true); } if(!vKeysCell.empty()) { for(vectorcv::KeyPoint::iterator vitvKeysCell.begin(); vit!vKeysCell.end();vit) { (*vit).pt.xj*wCell; (*vit).pt.yi*hCell; vToDistributeKeys.push_back(*vit); } } } } vectorKeyPoint keypoints allKeypoints[level]; keypoints.reserve(nfeatures); // 根据mnFeaturesPerLevel即该层的兴趣点数,对特征点进行剔除 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 i0; inkps ; i) { keypoints[i].pt.xminBorderX; keypoints[i].pt.yminBorderY; keypoints[i].octavelevel; keypoints[i].size scaledPatchSize; } } // compute orientations for (int level 0; level nlevels; level) computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); } void ORBextractor::ComputeKeyPointsOld(std::vectorstd::vectorKeyPoint 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); vectorvectorvectorKeyPoint cellKeyPoints(levelRows, vectorvectorKeyPoint (levelCols)); vectorvectorint nToRetain(levelRows,vectorint(levelCols,0)); vectorvectorint nTotal(levelRows,vectorint(levelCols,0)); vectorvectorbool bNoMore(levelRows,vectorbool(levelCols,false)); vectorint iniXCol(levelCols); vectorint iniYRow(levelRows); int nNoMore 0; int nToDistribute 0; float hY cellH 6; for(int i0; ilevelRows; i) { const float iniY minBorderY i*cellH - 3; iniYRow[i] iniY; if(i levelRows-1) { hY maxBorderY3-iniY; if(hY0) continue; } float hX cellW 6; for(int j0; jlevelCols; j) { float iniX; if(i0) { iniX minBorderX j*cellW - 3; iniXCol[j] iniX; } else { iniX iniXCol[j]; } if(j levelCols-1) { hX maxBorderX3-iniX; if(hX0) continue; } Mat cellImage mvImagePyramid[level].rowRange(iniY,iniYhY).colRange(iniX,iniXhX); cellKeyPoints[i][j].reserve(nfeaturesCell*5); FAST(cellImage,cellKeyPoints[i][j],iniThFAST,true); 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(nKeysnfeaturesCell) { nToRetain[i][j] nfeaturesCell; bNoMore[i][j] false; } else { nToRetain[i][j] nKeys; nToDistribute nfeaturesCell-nKeys; bNoMore[i][j] true; nNoMore; } } } // Retain by score while(nToDistribute0 nNoMorenCells) { int nNewFeaturesCell nfeaturesCell ceil((float)nToDistribute/(nCells-nNoMore)); nToDistribute 0; for(int i0; ilevelRows; i) { for(int j0; jlevelCols; j) { if(!bNoMore[i][j]) { if(nTotal[i][j]nNewFeaturesCell) { 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; } } } } } vectorKeyPoint keypoints allKeypoints[level]; keypoints.reserve(nDesiredFeatures*2); const int scaledPatchSize PATCH_SIZE*mvScaleFactor[level]; // Retain by score and transform coordinates for(int i0; ilevelRows; i) { for(int j0; jlevelCols; j) { vectorKeyPoint 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 k0, kendkeysCell.size(); kkend; k) { keysCell[k].pt.xiniXCol[j]; keysCell[k].pt.yiniYRow[i]; keysCell[k].octavelevel; 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, vectorKeyPoint keypoints, Mat descriptors, const vectorPoint 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)); } void ORBextractor::operator()( InputArray _image, InputArray _mask, vectorKeyPoint _keypoints, OutputArray _descriptors) { if(_image.empty()) return; Mat image _image.getMat(); assert(image.type() CV_8UC1 ); // Pre-compute the scale pyramid // 构建图像金字塔并包含边界EDGE_THRESHOLD ComputePyramid(image); // 计算每层图像的兴趣点 vector vectorKeyPoint allKeypoints; // vectorvectorKeyPoint 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) { vectorKeyPoint keypoints allKeypoints[level]; int nkeypointsLevel (int)keypoints.size(); if(nkeypointsLevel0) continue; // preprocess the resized image 对图像进行高斯模糊 Mat workingMat mvImagePyramid[level].clone(); 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 (vectorKeyPoint::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()); } } /** * 构建图像金字塔并包含边界EDGE_THRESHOLD * 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_101BORDER_ISOLATED); } else { copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, BORDER_REFLECT_101); } } }参考文献ORB-SLAM2从理论到代码实现三ORB特征提取和匹配理论和代码详解_波波菠菜的博客-CSDN博客ORB-SLAM2系列第二章——ORB 特征点提取_running snail szj的博客-CSDN博客_orb特征提取
