ORB-SLAM 解读(四) 单目初始化中特征匹配, 方向一致性检验

https://mp.weixin.qq.com/s/O2_wiwnWjiI2R4VVAzrbVQ

ORBSLAM2中的特征匹配

小白：师兄，ORB-SLAM2里有很多种特征匹配函数，看的我眼花缭乱，为什么有这么多种呢？怎么决定什么时候用哪一种特征匹配方法呢？

师兄：确实是比较多，但是基本思想是类似的，而且很多函数多次重载。我把特征匹配的几个重要函数都列出来，做了说明，见下：

// 通过词袋来搜索匹配，用于刚刚初始化后跟踪参考关键帧里的快速匹配
int SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)// 通过词袋来搜索匹配，用于闭环计算Sim3时候当前关键帧和闭环候选关键帧之间快速匹配
int SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)// 用于单目初始化时只在原图上进行的区域搜索匹配
int SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)// 通过词袋来搜索匹配，用于局部建图线程里两两关键帧之间尚未匹配特征点的快速匹配，为了生成新的匹配点对
int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12, vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)// 用于恒速模型跟踪，用前一个普通帧投影到当前帧进行匹配
int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)// 用于局部地图点跟踪，用所有局部地图点通过投影进行特征点匹配
int SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)// 用于闭环线程，将闭环关键帧及其共视关键帧的所有地图点投影到当前关键帧进行投影匹配
int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th)// 用于重定位跟踪，将候选关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配
int SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)// 用于闭环线程里Sim3变换，对当前关键帧和候选闭环关键帧互相投影匹配，生成更多的匹配点对
int SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint*> &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)// 用于局部建图线程，将地图点投影到关键帧中进行匹配和融合
int Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)// 用于闭环线程，将当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点投影到当前关键帧进行匹配融合
int Fuse(KeyFrame *pKF, cv::Mat Scw, const vector<MapPoint *> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint)

下面选择几个典型代码进行分析。

单目初始化中的特征匹配

师兄：单目初始化时，没有任何的先验信息如何进行特征匹配呢？

小白：我能想到的就是暴力匹配了。。。

师兄：暴力匹配缺点太多了，比如：

效率极低。单目初始化时提取的特征点是平时跟踪的好几倍，若采用暴力匹配计算量会指数级上升，无法接受。
效果不好。花那么大的代价去匹配，如果效果好也就忍了，可事与愿违，因为是“毫无目的”的匹配，所以误匹配非常多。

因此用“吃力不讨好”来形容暴力匹配太适合不过了。如果我们能找到一些先验信息，哪怕不那么准确的先验信息，加以利用，就能大大缓解上述情况。

在ORB-SLAM2的单目纯视觉初始化里的思路是这样的，参与初始化的两帧我们默认是比较接近的，也就是说，我们在第1帧提取的特征点坐标对应的第2帧里的位置附近画一个圆，匹配点应该就落在这个圆里。

如何快速确定候选匹配点？

师兄：上面提到了需要先确定候选匹配点，由于初始化时无法知晓相机的运动大小，所以确定候选匹配点时搜索圆半径会设置的相对比较大（代码里半径为100个像素）。你算下大概有多少像素？

小白：我简单算了下Π*r^2 = 31400 个像素，哇！这个区域挺大了啊！

师兄：是的，这样虽然比暴力匹配缩小了范围，但是索引的像素区域也是比较大的，如果逐像素匹配代价也非常大。

小白：ORB-SLAM2肯定采取了什么方法来加速吧？

师兄：是的，方法就是划分网格。这个在提取完特征点后就已经顺手将特征点划分为不同的网格并记录在mGrid里。在这里就派上了用场。如下图所示，搜索的时候是以网格为单位进行搜索的，代码里网格尺寸默认是64x48，这样圆里包含的网格数目就大大减少。具体过程如下：

我们先根据圆的范围确定圆的上下左右边界分别在哪个网格内。图中圆的边界在水平坐标轴上的坐标范围是mMinCellX~mMaxCellX，在垂直坐标轴上的坐标范围是mMinCellY~mMaxCellY。mnMinX是图像的左侧边界对应的像素坐标。
遍历圆形区域内所有的网格。如果某个网格内没有特征点，那么直接跳过；如果某个网格内有特征点（见下图中绿色方框），遍历这些特征点，判断这些特征点是否符合要求的金字塔层级，是否在圆内，如果满足条件会把该特征点作为候选特征点。这种方式大大提高了搜索效率。
快速搜索候选匹配点的代码见：

/*** @brief 找到在 以x,y为中心,半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的候选特征点* * @param[in] x                     特征点坐标x* @param[in] y                     特征点坐标y* @param[in] r                     搜索半径 * @param[in] minLevel              最小金字塔层级* @param[in] maxLevel              最大金字塔层级* @return vector<size_t>           返回搜索到的候选匹配点id*/
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const
{// 存储搜索结果的vectorvector<size_t> vIndices;vIndices.reserve(N);// Step 1 计算半径为r圆左右上下边界所在的网格列和行的id// 查找半径为r的圆左侧边界所在网格列坐标。这个地方有点绕，慢慢理解下：// (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素（肯定大于1）// mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数，表示每个像素可以均分几个网格列（肯定小于1）// (x-mnMinX-r)，可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数// 两者相乘，就是求出那个半径为r的圆的左侧边界在哪个网格列中// 保证nMinCellX 结果大于等于0const int nMinCellX = max(0,(int)floor( (x-mnMinX-r)*mfGridElementWidthInv));// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vectorif(nMinCellX>=FRAME_GRID_COLS)return vIndices;// 计算圆所在的右边界网格列索引const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));// 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vectorif(nMaxCellX<0)return vIndices;//后面的操作也都是类似的，计算出这个圆上下边界所在的网格行的idconst int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));if(nMinCellY>=FRAME_GRID_ROWS)return vIndices;const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));if(nMaxCellY<0)return vIndices;// 检查需要搜索的图像金字塔层数范围是否符合要求const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);// Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里for(int ix = nMinCellX; ix<=nMaxCellX; ix++){for(int iy = nMinCellY; iy<=nMaxCellY; iy++){// 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引const vector<size_t> vCell = mGrid[ix][iy];// 如果这个网格中没有特征点，那么跳过这个网格继续下一个if(vCell.empty())continue;// 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点for(size_t j=0, jend=vCell.size(); j<jend; j++){// 根据索引先读取这个特征点 const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];// 保证给定的搜索金字塔层级范围合法if(bCheckLevels){// cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据// 保证特征点是在金字塔层级minLevel和maxLevel之间，不是的话跳过if(kpUn.octave<minLevel)continue;if(maxLevel>=0)  if(kpUn.octave>maxLevel)continue;}               // 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内const float distx = kpUn.pt.x-x;const float disty = kpUn.pt.y-y;// 如果x方向和y方向的距离都在指定的半径之内，存储其index为候选特征点if(fabs(distx)<r && fabs(disty)<r)vIndices.push_back(vCell[j]);}}}return vIndices;
}

方向一致性检验

师兄：经过上面条件1、2的检验后，我们还要做方向一致性检验。因为通过特征点匹配后的结果仍然可能不一定准确，我们需要剔除掉其中的错误匹配。原理是统计两张图像所有匹配对中两个特征点主方向的差，构建一个直方图。由于两张图像整体发了运动，所以特征点匹配对主方向整体会有一个统一一致的变化，我们认为直方图里前三个最大的格子（代码里称为bin）里就是正常的匹配点对，那些误匹配的特征点对此时就会暴露了，会落在直方图上述之外的其他格子里，这些就是需要剔除的错误匹配。

我们举个可能不是很恰当的例子来类比一下，军训时所有人都站的笔直，教官下令“向左转”，此时大部分人都能正确的向左转90°左右，当然大家转动的幅度可能不同，但基本都朝着同一个方向旋转了80°~100°左右，这就相当于我们前面说的直方图里前三个最大的格子。但可能会有少量分不清左右的人向右转或者干脆不转，此时他们相对之前的旋转角度可能是-90°或0°，因为这样的人很少，对应直方图频率就很低，角度变化就不在直方图前三大的格子里，就是需要剔除的对象。

下图（a）是初始的特征匹配对，（b）是经过方向一致性验证后删除的特征匹配对。我们可以看到明显错误匹配对被剔除了

这部分代码见：

/*** @brief 单目初始化中用于参考帧和当前帧的特征点匹配* 步骤* Step 1 构建旋转直方图* Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 * Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的* Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配* Step 5 根据设定阈值剔除误匹配，统计角度变化直方图* Step 6 根据上面直方图统计信息剔除误匹配的特征点对* Step 7 将最后通过筛选的匹配好的特征点保存* @param[in] F1                        初始化参考帧                  * @param[in] F2                        当前帧* @param[in & out] vbPrevMatched       本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标* @param[in & out] vnMatches12         保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引* @param[in] windowSize                搜索窗口* @return int                          返回成功匹配的特征点数目*/
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
{int nmatches=0;// F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);// Step 1 构建旋转直方图，HISTO_LENGTH = 30vector<int> rotHist[HISTO_LENGTH];for(int i=0;i<HISTO_LENGTH;i++)rotHist[i].reserve(500);   //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   const float factor = HISTO_LENGTH/360.0f;// 匹配点对距离，注意是按照F2特征点数目分配空间vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);// 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间vector<int> vnMatches21(F2.mvKeysUn.size(),-1);// 遍历帧1中的所有特征点for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++){cv::KeyPoint kp1 = F1.mvKeysUn[i1];int level1 = kp1.octave;// 只使用原始图像上提取的特征点if(level1>0)continue;// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 // vbPrevMatched 输入的是参考帧 F1的特征点// windowSize = 100，输入最大最小金字塔层级 均为0vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);// 没有候选特征点，跳过if(vIndices2.empty())continue;// 取出参考帧F1中当前遍历特征点对应的描述子cv::Mat d1 = F1.mDescriptors.row(i1);int bestDist = INT_MAX;     //最佳描述子匹配距离，越小越好int bestDist2 = INT_MAX;    //次佳描述子匹配距离int bestIdx2 = -1;          //最佳候选特征点在F2中的索引// Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){size_t i2 = *vit;// 取出候选特征点对应的描述子cv::Mat d2 = F2.mDescriptors.row(i2);// 计算两个特征点描述子距离int dist = DescriptorDistance(d1,d2);if(vMatchedDistance[i2]<=dist)continue;// 如果当前匹配距离更小，更新最佳次佳距离if(dist<bestDist){bestDist2=bestDist;bestDist=dist;bestIdx2=i2;}else if(dist<bestDist2){bestDist2=dist;}}// Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配// 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值if(bestDist<=TH_LOW){// 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高if(bestDist<(float)bestDist2*mfNNratio){// 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉if(vnMatches21[bestIdx2]>=0){vnMatches12[vnMatches21[bestIdx2]]=-1;nmatches--;}// 次优的匹配关系，双向建立// vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引vnMatches12[i1]=bestIdx2;vnMatches21[bestIdx2]=i1;vMatchedDistance[bestIdx2]=bestDist;nmatches++;// Step 5 计算匹配点旋转角度差所在的直方图if(mbCheckOrientation){// 计算匹配特征点的角度差，这里单位是角度°，不是弧度float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;if(rot<0.0)rot+=360.0f;// bin 表示当前rot被分配在第几个直方图盒子 int bin = round(rot*factor);// 如果bin 满了又是一个轮回if(bin==HISTO_LENGTH)bin=0;assert(bin>=0 && bin<HISTO_LENGTH);rotHist[bin].push_back(i1);}}}}// Step 6 根据上面直方图统计信息剔除误匹配的特征点对if(mbCheckOrientation){int ind1=-1;int ind2=-1;int ind3=-1;// 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);for(int i=0; i<HISTO_LENGTH; i++){// 如果特征点的旋转角度变化量属于这三个组，说明符合整体运动趋势，属于内点匹配对if(i==ind1 || i==ind2 || i==ind3)continue;// 否则属于外点匹配对，需要剔除，因为他们和整体运动趋势有较大偏差   for(size_t j=0, jend=rotHist[i].size(); j<jend; j++){int idx1 = rotHist[i][j];if(vnMatches12[idx1]>=0){vnMatches12[idx1]=-1;nmatches--;}}}}// Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatchedfor(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)if(vnMatches12[i1]>=0)vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;return nmatches;
}

ORB-SLAM 解读(四) 单目初始化中特征匹配, 方向一致性检验相关推荐

超详细解读ORB-SLAM3单目初始化（下篇）
一前言本文承接ORB-SLAM3 细读单目初始化过程(上),ORBSLAM3单目视觉有很多知识点需要展开和深入,初始化过程是必然要经历的,而网上资料不够系统,因此本文主旨是从代码实现出发,把初始化 ...
超详细解读ORB-SLAM3 单目初始化过程（上篇）
学习ORB-SLAM3单目视觉SLAM中,发现有很多知识点需要展开和深入,同时又需要对系统有整体的认知,为了强化记忆,记录该系列笔记,为自己图方便,也希望对大家有所启发. 因为知识有限,因此先记录初始 ...
ORB_SLAM2 源码解析单目初始化器Initializer(三)
目录一.地图点初始化二.重新记录特征点的匹配关系 1.构建旋转直方图 1.1.在半径窗口内搜索当前帧F2中所有的候选匹配特征点GetFeaturesInArea 1.2.表示一个图像像素相当于多少 ...
ORB-SLAM3 细读单目初始化过程（终结篇）
本文原创,转载请说明地址:https://blog.csdn.net/shanpenghui/article/details/110522368 一.前言请阅读本文之前最好把ORB-SLAM3的单目 ...
ORB_SLAM2单目初始化策略
基本流程单目初始化程序存储在Initializer.cc中需要注意,对于双目/RGB-D相机,初始化时,由于可以直接获得相机的深度信息,因此无需求H/F,直接作为关键帧插入就行. 使用 ...
ORBSLAM源码理论分析2—单目初始化
ORBSLAM源码理论分析2-单目初始化 1.构造初始化帧1 2.第一次初始化 3.构造初始化帧2 4.F1与F2特征匹配 5.初始化解算位姿 5.1.计算单应矩阵 5.2.计算基础矩阵 5.3.评估 ...
单目初始化单应矩阵本质矩阵恢复R t 三角变换求 3D点
单目初始化单应矩阵本质矩阵恢复R t 三角变换求 3D点博文末尾支持二维码赞赏哦 ^_^ /* * This file is part of ORB-SLAM2 * * 单目相机初始化 * 用 ...
重磅直播|ORB-SLAM3经典单目初始化模块原理及实现
点击上方"计算机视觉工坊",选择"星标" 干货第一时间送达大家好,本公众号现已开启线上视频公开课,主讲人通过B站直播间,对3D视觉领域相关知识点进行讲解,并在 ...
【ORB-SLAM2源码梳理6】Track()函数的第一步：单目初始化MonocularInitialization()
文章目录前言一.Track()函数二.单目初始化MonocularInitialization() 1. 判断单目初始化器是否创建,若没有就创建. 2. 已创建初始化器,判断特征点数目 3. 在 ...
ORB-SLAM3 细读单目初始化过程（下）
本文原创,转载请说明地址:https://blog.csdn.net/shanpenghui/article/details/110003959 一.前言 ORBSLAM3单目视觉有很多知识点需要展开 ...

ORB-SLAM 解读(四) 单目初始化中特征匹配, 方向一致性检验

ORB-SLAM 解读(四) 单目初始化中特征匹配, 方向一致性检验相关推荐

最新文章

热门文章