orb-slam2代码总结（四）特征点匹配

orb-slam作为一款优秀的特征点slam方法，其特征点匹配好坏相当重要，直接关系到建图和定位的准确度。

1、特征点提取

orb特征点是对fast角点的改进，通过加入尺度和旋转的属性，使得orb特征点具备了尺度和旋转不变性.

1.1 特征点数目

每一金字塔层选取特征点理论总数目nfeatures，随着金字塔层数越高（分辨率越低），按照等比数列下降.

1.2 fast角点

在提取特征之前，先要获取原图像的金字塔层，一般划分8个左右的层，通过调用ComputePyramid()函数，使用resize()函数中的双线性插值方式进行降采样，存储在mvImagePyramid变量中，这期间没有进行高斯模糊，后面提取描述子的才会用到高斯模糊.

1）对每一层图像划分30*30的栅格，在每个栅格内提取FAST角点，保证了角点分布的均匀，根据这一层的nfeatures使用DistributeOctTree()进行剔除. 而opencv自带的orb特征点检测，可能会造成特征点过于集中，这样对于tracking很不利，很容易就跟丢了.

orb-slam2代码总结（四）特征点匹配

2）为了使fast角点具备旋转属性，使用IC_Angle()（灰度质心法）计算关键点的角度，选用的图像块PATCH_SIZE大小是31个像素，u_max是事先根据PATCH_SIZE计算的v在圆周上对应u的大小，代码实现也很简单，如下：

for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        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); //计算上下的时候是有符号的，所以是减val_minus
            m_10 += u * (val_plus + val_minus); //将(u,v)和(u,-v)两个点的像素一起计算，这边是m_10加，是由于u已经确定好了符号
        }
        m_01 += v * v_sum;
    }

3）关键点剔除，使用4叉树

1.3 描述子

计算完关键点之后，既可以在每一个关键点附近计算描述子矩阵了，首先对各层图像进行高斯模糊GaussianBlur()，选取规则使用是一个静态数组，根据关键点的角度，对pattern对进行转换，然后再对转换后的这两个像素值的大小进行比较，一共计算256维，存到包含32个char类型的矩阵中

//使得该关键点(兴趣点和描述子)具有旋转不变性，因为都映射到同一个xy坐标系，计算出来的描述子当然也就具有旋转不变性
    #define GET_VALUE(idx) \
        center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
               cvRound(pattern[idx].x*a - pattern[idx].y*b)]

2、特征点匹配

ORB-SLAM2中特征点匹配均采用了各种技巧减小特征点匹配范围，特征点通过描述子匹配后会进行旋转一致性检测，并且要求最佳匹配特征点要明显优于次优匹配点.

2.1 MonocularInitialization() 中的 SearchForInitialization()

SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, 
vector<int> &vnMatches12, int windowSize)

1）搜索范围
搜索尺度保持与上一帧同层，并且只对mInitialFrame和mCurrentFrame中的第0层特征点进行匹配，在窗口范围内，使用<上一帧的关键点坐标>在<当前帧该坐标>附近进行同层搜索，即下面代码里的vbPrevMatched[i1].x和vbPrevMatched[i1].y 是来自mInitialFrame，使用这个坐标在mCurrentFrame 周围windowSize半径范围内搜索

vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, 
windowSize,level1,level1);

2）候选关键点搜索
使用的是 GetFeaturesInArea() 函数，搜索半径范围由windowSize指定，找到以x、y为中心，边长为2r的方形内且在[minLevel, maxLevel]的特征点.
这里有一个加速寻找的trick，就是将整幅图像的所有特征点栅格化了，即通过frame构造函数里的AssignFeaturesToGrid()函数将特征点划分到对应的栅格内，这样，当寻找windowSize附近的特征点时，可以缩小范围. 主要功能代码如下

for(int ix = nMinCellX; ix<=nMaxCellX; ix++)                                
{                                                                           
    for(int iy = nMinCellY; iy<=nMaxCellY; iy++)                            
    {                                                                       
        const vector<size_t> vCell = mGrid[ix][iy]; //取出该格子里的特征点ID vector   
        if(vCell.empty())                                                   
            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]); //压栈                          
        }                                                                   
    }                                                                       
}

3）比例阈值进行筛选
最佳匹配比次佳匹配明显要好（使用mfNNratio进行控制），那么最佳匹配才真正靠谱，避免出现模棱两可、似是而非这种情况，mfNNratio通过ORBmatcher matcher(0.7,true);在构造时指定，值越小匹配越苛刻，根据情况选0.9或者0.7等

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）角度投票（旋转一致性）进行剔除误匹配
每个特征点在提取描述子时的旋转主方向角度，如果图像旋转了，这个角度将发生改变，所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值

if(mbCheckOrientation)                                         
{                                                                                                                              
    float rot = kp.angle-F.mvKeys[bestIdxF].angle;// 该特征点的角度变化值
    if(rot<0.0)                                                
        rot+=360.0f;                                           
    int bin = round(rot*factor);// 将rot分配到bin组                 
    if(bin==HISTO_LENGTH)                                      
        bin=0;                                                 
    assert(bin>=0 && bin<HISTO_LENGTH);                        
    rotHist[bin].push_back(bestIdxF);                          
}

2.2 TrackReferenceKeyFrame() 中的 SearchByBoW()

int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)

通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配）
对属于同一node的特征点通过描述子距离进行匹配（暴力匹配）
根据匹配，用pKF中特征点对应的MapPoint更新F中特征点对应的MapPoints
每个特征点都对应一个MapPoint，因此pKF中每个特征点的MapPoint也就是F中对应点的MapPoint
通过距离阈值(描述子距离)、比例阈值（最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱）和角度投票（旋转一致性）进行剔除误匹配

2.3 TrackWithMotionModel() 中的 SearchByProjection()

将上一帧的MapPoints投影到当前帧(根据速度模型可以估计当前帧的Tcw)
在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
使用的候选关键点搜索函数：GetFeaturesInArea
搜索的尺度，根据前进还是后退设定
搜索范围（半径，windowSize），根据th、与特征点在上一帧时的尺度

2.4 TrackLocalMap()、SearchLocalPoints()，通过SearchByProjection()：

SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)

将Local MapPoint投影到当前帧中, 由此增加当前帧的MapPoints \n
在SearchLocalPoints()中已经将Local MapPoints重投影（isInFrustum()）到当前帧 \n
并标记了这些点是否在当前帧的视野中，即mbTrackInView \n
使用的候选关键点搜索函数：GetFeaturesInArea
对这些MapPoints，在其投影点附近根据描述子距离选取匹配，
以及最终的方向投票机制进行剔除

5、Relocalization
DetectRelocalizationCandidates

1. 找出和当前帧具有公共单词的所有关键帧，使用反向索引
1. 只和具有共同单词较多的关键帧进行相似度计算
1. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
1. 只返回<累计得分较高的组>中分数最高的关键帧