ORB_SLAM2原理及代码解析：ORBmatcher::SearchForInitialization() 函数
1 作用
用参考帧 F1 和当前帧 F2 做特征点匹配，返回匹配数量和匹配关系。
2 参数说明
3 代码
（1）声明：ORBmatcher.h
// Matching for the Map Initialization (only used in the monocular case)int SearchForInitialization(Frame &F1, Frame &F2, std::vector<cv::Point2f> &vbPrevMatched, std::vector<int> &vnMatches12, int windowSize=10);（2）定义：ORBmatcher.cc
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize) {int nmatches=0;vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);vector<int> rotHist[HISTO_LENGTH];for(int i=0;i<HISTO_LENGTH;i++)rotHist[i].reserve(500);const float factor = 1.0f/HISTO_LENGTH;vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);vector<int> vnMatches21(F2.mvKeysUn.size(),-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;vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);if(vIndices2.empty())continue;cv::Mat d1 = F1.mDescriptors.row(i1);int bestDist = INT_MAX;int bestDist2 = INT_MAX;int bestIdx2 = -1;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;}}if(bestDist<=TH_LOW){if(bestDist<(float)bestDist2*mfNNratio){if(vnMatches21[bestIdx2]>=0){vnMatches12[vnMatches21[bestIdx2]]=-1;nmatches--;}vnMatches12[i1]=bestIdx2;vnMatches21[bestIdx2]=i1;vMatchedDistance[bestIdx2]=bestDist;nmatches++;if(mbCheckOrientation){float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;if(rot<0.0)rot+=360.0f;int bin = round(rot*factor);if(bin==HISTO_LENGTH)bin=0;assert(bin>=0 && bin<HISTO_LENGTH);rotHist[bin].push_back(i1);}}}}if(mbCheckOrientation){int ind1=-1;int ind2=-1;int ind3=-1;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--;}}}}//Update prev matchedfor(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)if(vnMatches12[i1]>=0)vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;return nmatches; }
4 代码解析
4.1 初始化
int nmatches=0;//存放参考帧F1的每个特征点在当前帧F2中的匹配结果，//初始化时另vnMatches12数组大小和F1去畸变的大小一致，且全都置为-1，表示未匹配vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);//旋转一致性直方图vector<int> rotHist[HISTO_LENGTH];for(int i=0;i<HISTO_LENGTH;i++)//rotHist[i],i即为bin值。给rotHist[i]分配了500个元素存储空间rotHist[i].reserve(500);//算出单位bin，如匹配点方向差rot=180，rot*factor得出的个位数即为这个匹配点落入的binconst float factor = 1.0f/HISTO_LENGTH;//保证F2的一个点只能被匹配一次，并且保留最优（最小距离）的匹配。vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);//保存F2→F1的匹配对应关系，辅助双向一致性检查。vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
4.1.1 旋转一致性直方图
vector<int> rotHist[HISTO_LENGTH];
rotHist是一个数组，长度为HISTO_LENGTH（ORB-SLAM 默认 = 30）。例子：
假设
HISTO_LENGTH = 30，即每个 bin = 12°匹配对 (F1[10], F2[200]) 的方向差 = 25°
→ 落入 bin = 25 / 12 ≈ 2
→rotHist[2].push_back(10)
PS.旋转一致性检查是为了剔除误匹配的点（相机只是平移或小幅旋转，所有匹配点的方向差会集中在某个角度范围）
4.2 遍历F1关键点
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++){cv::KeyPoint kp1 = F1.mvKeysUn[i1];//octave为F1金字塔的层数int level1 = kp1.octave;if(level1>0)continue;
4.3 F2中搜索候选点
vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);if(vIndices2.empty())continue;GetFeaturesInArea() 函数如下
4.3.1 Frame::GetFeaturesInArea() 函数
4.3.1.1 声明：Frame.h
vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1) const;
4.3.1.2 定义:Frame.cc
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const {vector<size_t> vIndices;vIndices.reserve(N);const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));if(nMinCellX>=FRAME_GRID_COLS)return vIndices;const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));if(nMaxCellX<0)return vIndices;const 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);for(int ix = nMinCellX; ix<=nMaxCellX; ix++){for(int iy = nMinCellY; iy<=nMaxCellY; iy++){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){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;if(fabs(distx)<r && fabs(disty)<r)vIndices.push_back(vCell[j]);}}}return vIndices; }
4.3.1.3 函数解析
（1）预分配空间
//vIndices 用来存储满足条件的特征点索引vector<size_t> vIndices;vIndices.reserve(N);（2）计算查询区域在网格中的范围
//搜索范围覆盖的最左边的格子编号 const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv)); //如果最左边的格子列索引大于最右边的，则返回，表示没有找到候选特征点 if(nMinCellX>=FRAME_GRID_COLS)return vIndices;//搜索最右边 const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv)); if(nMaxCellX<0)return vIndices;const 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;（3）判断是否要金字塔层数过滤
const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);（4）遍历候选格子，筛选点
for(int ix = nMinCellX; ix<=nMaxCellX; ix++) {for(int iy = nMinCellY; iy<=nMaxCellY; iy++){//取出格子里特征点const vector<size_t> vCell = mGrid[ix][iy];if(vCell.empty())continue;（5）对每个格子里的点检查
for(size_t j=0, jend=vCell.size(); j<jend; j++) {const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];if(bCheckLevels){//如果当前特征点所在层数小于最小层，则跳过if(kpUn.octave<minLevel)continue;if(maxLevel>=0)if(kpUn.octave>maxLevel)continue;}//计算候选特征点kpUn.pt.x与搜索中心x的水平和垂直(y)距离const float distx = kpUn.pt.x-x;const float disty = kpUn.pt.y-y;//|distx| < r → 横向距离在半径范围内if(fabs(distx)<r && fabs(disty)<r)vIndices.push_back(vCell[j]); }（6）返回结果
return vIndices;
4.3.2 Frame::GetFeaturesInArea() 函数
（1）声明：KeyFrame.h
// KeyPoint functionsstd::vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r) const;（2）定义：KeyFrame.cc
vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const {vector<size_t> vIndices;vIndices.reserve(N);const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));if(nMinCellX>=mnGridCols)return vIndices;const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));if(nMaxCellX<0)return vIndices;const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));if(nMinCellY>=mnGridRows)return vIndices;const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));if(nMaxCellY<0)return vIndices;for(int ix = nMinCellX; ix<=nMaxCellX; ix++){for(int iy = nMinCellY; iy<=nMaxCellY; iy++){const vector<size_t> vCell = mGrid[ix][iy];for(size_t j=0, jend=vCell.size(); j<jend; j++){const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];const float distx = kpUn.pt.x-x;const float disty = kpUn.pt.y-y;if(fabs(distx)<r && fabs(disty)<r)vIndices.push_back(vCell[j]);}}}return vIndices; }
4.4 计算描述子距离，找最佳匹配
cv::Mat d1 = F1.mDescriptors.row(i1);int bestDist = INT_MAX;int bestDist2 = INT_MAX;int bestIdx2 = -1;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;}}
4.5 NN ratio test + 一致性检查
if(bestDist<=TH_LOW){if(bestDist<(float)bestDist2*mfNNratio){//判断候选点是否已被匹配if(vnMatches21[bestIdx2]>=0){vnMatches12[vnMatches21[bestIdx2]]=-1;nmatches--;}vnMatches12[i1]=bestIdx2;vnMatches21[bestIdx2]=i1;vMatchedDistance[bestIdx2]=bestDist;nmatches++;
4.6 旋转一致性直方图
if(mbCheckOrientation){float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;if(rot<0.0)rot+=360.0f;int bin = round(rot*factor);if(bin==HISTO_LENGTH)bin=0;assert(bin>=0 && bin<HISTO_LENGTH);rotHist[bin].push_back(i1);}
4.7 过滤旋转不一致的匹配
if(mbCheckOrientation){int ind1=-1;int ind2=-1;int ind3=-1;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--;}}}}
4.8 更新
//Update prev matchedfor(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)if(vnMatches12[i1]>=0)vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
4.9 返回匹配数
return nmatches;