ORBSLAM2源码
ORB-SLAM2-单目初始化中的特征点匹配
一、代码
在单目初始化完成后,下面就开始对单目初始化中的两帧进行匹配
单目初始化中特征点的匹配函数在 SearchForInitialization()中;
进入 SearchForInitialization()
函数中
//这个函数在ORBmatcher.cc这个文件中
int ORBmatcher::(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)//这个函数传入的参数是,初始帧图像 、当前帧的图像、当前帧的keypoints坐标、匹配vector 、窗口大小
{
int nmatches=0;//定义匹配点的个数
vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);//将存储匹配点的容器大小定为F1帧中keypoints的数目大小,-1表示这里面含没有存储匹配点,存储的是F1与F2进行匹配
//构建旋转直方图
vector<int> rotHist[HISTO_LENGTH];//定义直方图,直方图大小为30
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);//在每个直方图小块中resize500大小
const float factor = 1.0f/HISTO_LENGTH;
vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);//两个keypoints的匹配距离,是按照F2中的keypoints的数量大小来进行分配的
vector<int> vnMatches21(F2.mvKeysUn.size(),-1);//进行反向匹配
//开始遍历F1中的所有特征点
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
{
cv::KeyPoint kp1 = F1.mvKeysUn[i1];//将F1中的去畸变后的特征点传给kp1
int level1 = kp1.octave;//获取当前帧中特征点所在的图层
if(level1>0)//如果图层大于0,那么就 continue。也就是只提取第0层图像中的特征点
continue;
vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);//存储的是窗口内的keypoints,F2中的候选匹配特征点
if(vIndices2.empty())//如果是空的话就continue
continue;
cv::Mat d1 = F1.mDescriptors.row(i1);//取出古F中的特侦点描述子
int bestDist = INT_MAX;//最佳描述子匹配距离
int bestDist2 = INT_MAX;//次佳描述子匹配距离
int bestIdx2 = -1;//最佳候选特征点在F2中索引
for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{//开始遍历F2中的候选keypoints
size_t i2 = *vit;//对指向F2中候选匹配点进行解指针
cv::Mat d2 = F2.mDescriptors.row(i2);//取出F2中的描述子
int dist = DescriptorDistance(d1,d2);//计算两个描述子的距离
if(vMatchedDistance[i2]<=dist)//如果dist比INT_MAX最大还要大的话,要了也没啥用,直接continue
continue;
if(dist<bestDist)//如果它的距离是小于最佳匹配距离的
{
bestDist2=bestDist;//将最佳距离给次佳距离
bestDist=dist;//将当前距离给最佳距离
bestIdx2=i2;//更新最佳匹配距离的索引ID
}
else if(dist<bestDist2)//如果距离在大于最佳距离 ,小于次佳距离,则更新次佳距离
{
bestDist2=dist;
}
}
//对最优结果进行检查,满足阈值,最优/次优的比例,删除重复匹配
if(bestDist<=TH_LOW)//也就是最优距离要小于这个阈值,配置文件中是50;
{
if(bestDist<(float)bestDist2*mfNNratio)//最佳/次佳的比例要小于0.9
{
if(vnMatches21[bestIdx2]>=0)//如果最佳匹配点的ID在vnMatches21中>0到说明,他已经被匹配过了
{
vnMatches12[vnMatches21[bestIdx2]]=-1;//那么将12中的值也置为-1,匹配的点数少一
nmatches--;
}
vnMatches12[i1]=bestIdx2;//这里面的值存的是F2中best的keypointsID
vnMatches21[bestIdx2]=i1;//反过来
vMatchedDistance[bestIdx2]=bestDist;//最佳匹配距离是这个
nmatches++;//匹配点加1
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)//如果等于30,那么就循环回来
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);//判断bin是否符合要求
rotHist[bin].push_back(i1);//将特侦点的idnex存入
}
}
}
if(mbCheckOrientation)//删除旋转直方图中非主流部分
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);//算出旋转角度差落在bin中最多的前三位的索引
for(int i=0; i<HISTO_LENGTH; i++)//遍历每个直方图
{
if(i==ind1 || i==ind2 || i==ind3)//如果在三个中就continue
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 matched
for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)//输出匹配成功的点
if(vnMatches12[i1]>=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
}
}
进入GetFeaturesInArea()的函数,在Frame.cc中
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const//传入的参数:F1中的keypoints的坐标。窗口的半径,最大最小金字塔层数,在单目初始化中是第0层
vector<size_t> vIndices;//存储搜索结果的vector
vIndices.reserve(N);//将他ressrve成特征点个数大小
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));//求出特征点窗口左边界是在那个表格中
if(nMinCellX>=FRAME_GRID_COLS)//如果它的最左侧边界格子的id大于64那么就返回退出
return vIndices;
const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));//定义keypoints最右侧的边界在哪一个格子中
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];//获取网格中特征点,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)//保证在第0层,不是的话跳过
{
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]);
}//在的话就存到Vindices中
}
}
return vIndices;
}
计算bin中最多的前三位,进入ComputeThreeMaxima()函数
void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)//传参:直方图,直方图长度,输出前三的索引
{
int max1=0;
int max2=0;
int max3=0;
for(int i=0; i<L; i++)//遍历每个bin
{
const int s = histo[i].size();//s=每个直方图的size
if(s>max1)
{//给S最大的值
max3=max2;
max2=max1;
max1=s;
ind3=ind2;
ind2=ind1;
ind1=i;
}
else if(s>max2)
{
max3=max2;
max2=s;
ind3=ind2;
ind2=i;
}
else if(s>max3)
{
max3=s;
ind3=i;
}
}
if(max2<0.1f*(float)max1)//差距太大都集中在1中,2.3都不要了
{
ind2=-1;
ind3=-1;
}
else if(max3<0.1f*(float)max1)
{
ind3=-1;
}
}