ORB-SLAM2-单目初始化中的特征点匹配

ORBSLAM2源码

---

---

一、代码

在单目初始化完成后，下面就开始对单目初始化中的两帧进行匹配

单目初始化中特征点的匹配函数在 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;
    }
}