ORB-SLAM2代码（四）特征点匹配

参考：orb-slam2源码注释版

orb-slam作为一款优秀的特征点slam方法，其特征点匹配好坏相当重要，直接关系到建图和定位的准确度。orb-slam中针对各种情况，设计了各种trick，以加速特征匹配，所以有必要学习下这些trick是如何使用的.

---

1、特征点提取

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

1.1 特征点数目

每幅图像提取的特征点最大数目由nfeatures设置（比如，设置1000，但不表示实际测试每幅图像都能提取这么多特征点，达到300就不错了），而每一金字塔层选取特征点数目，随着金字塔层数越高（分辨率越低），按照等比系数1.0f/scaleFactor下降.

1.2 fast角点

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

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

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叉树
@todo

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()

int ORBmatcher::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)

1）使用词袋模型进行特征点匹配
使用词袋模型（BoW）进行特征点匹配使用的是direct index正向索引，反向索引inverse index则用于重定位和闭环检测中. 关于词袋模型，我放到另一篇博客单独介绍.

通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配），对属于同一node的特征点通过描述子距离进行匹配（暴力匹配，小范围内使用还是可以的），根据匹配，用pKF中特征点对应的MapPoint更新F中特征点对应的MapPoints.

2）筛选trick
同上，通过距离阈值(描述子距离)、比例阈值（最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱）和角度投票（旋转一致性）进行剔除误匹配.

2.3 TrackWithMotionModel() 中的 SearchByProjection()

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)

根据速度模型可以估计当前帧的Tcw，将上一帧的MapPoints投影到当前帧，使用函数 GetFeaturesInArea() 搜索候选特征点，搜索范围（半径，windowSize），根据th和与特征点在上一帧时的尺度nLastOctave共同确定，搜索使用的最大最小层，根据前进和后退模型（仅对双目和RGBD有效）设定，实现代码如下：

int nLastOctave = LastFrame.mvKeys[i].octave; /** @attention 特征点出现在上一帧时的尺度*/              
                                                                                          
// Search in a window. Size depends on scale                                                                                                          
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大                
                                                                                          
vector<size_t> vIndices2;                                                                 
                                                                                                                                                               
/** @attention */                                                                         
// NOTE 尺度越大,图像越小                                                                         
// 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点                                                   
// 当前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来                                    
// 因此m>=n，对应前进的情况，nCurOctave>=nLastOctave。后退的情况可以类推                                       
if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave                                   
    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);                 
else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave                          
    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);              
else // 在[nLastOctave-1, nLastOctave+1]中搜索                                                
    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);

得到候选特征点之后，在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除，同上.
对于双目还需要检查在右相机像素坐标系下，MapPoint在当前帧上投影位置与候选特征点u方向的距离是否是radius范围内.

if(CurrentFrame.mvuRight[i2]>0)                                        
{                                                                      
    // 双目和rgbd的情况，需要保证右图的点也在搜索半径以内                                     
    const float ur = u - CurrentFrame.mbf*invzc;                       
    const float er = fabs(ur - CurrentFrame.mvuRight[i2]); /** @todo */
    if(er>radius)                                                      
        continue;                                                      
}                                                                      

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

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

只对除了 TrackReferenceKeyFrame() 函数中匹配上的MapPoint以外的local MapPoint进行 SearchByProjection()，以向mCurrentFrame增加新的MapPoint
注意，MapPoint与当前帧上的特征点进行匹配时，使用的是该MapPoint的最适合的描述子（通过函数 ComputeDistinctiveDescriptors() 计算得到，先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值）

1）计算当前视角和平均视角夹角的余弦值
Local MapPoint这么多，究竟选择那些进行筛选呢，当然是选择能投影到当前帧中那些mapPoint，使用函数 isInFrustum() 完成这个工作，通过计算<该MapPoint与当前帧的相机中心连线>与该MapPoint的平均观测法向量mNormalVector夹角的余弦值，来判断当前帧是否能看到该MapPoint，阈值选择为0.5，即表示小于cos(60)，夹角大于60度就pass掉MapPoint.

// Check viewing angle                                   
// V-D 2) 计算当前视角和平均视角夹角的余弦值, 若小于cos(60), 即夹角大于60度则返回     
cv::Mat Pn = pMP->GetNormal();                           
                                                         
const float viewCos = PO.dot(Pn)/dist;                   
                                                         
if(viewCos<viewingCosLimit)                              
    return false;                                        
                                                         
// Predict scale in the image                            
// V-D 4) 根据深度预测尺度（对应特征点在一层）                             
const int nPredictedLevel = pMP->PredictScale(dist,this);

2）根据深度估计MapPoint在当前帧下所属的层
根据该MapPoint在当前帧中，距离相机光心的距离进行判断，代码如下：

int MapPoint::PredictScale(const float &currentDist, Frame* pF)
{
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        ratio = mfMaxDistance/currentDist;
    }

    int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pF->mnScaleLevels)
        nScale = pF->mnScaleLevels-1;

    return nScale;
}

3）候选关键点搜索
使用函数 GetFeaturesInArea() 搜索候选特征点，搜索范围（注意跟上面几种方式的区别），根据1)中计算的夹角和2)中估计的层共同决定：

• 夹角越小，范围越小，代码如下
• 层越高，范围越大

float ORBmatcher::RadiusByViewingCos(const float &viewCos)
{
    if(viewCos>0.998)
        return 2.5;
    else
        return 4.0;
}

最后根据比例阈值（最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱）进行帅选，同上.

2.5 CreateNewMapPoints() 中的 SearchForTriangulation()

int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)

因为在local mapping线程中，关键帧的位姿已经相当准确了，即F12也是比较准确的，所以可以使用三角化生成新的MapPoint，匹配使用的是BoW，筛选部分使用了对极点邻域剔除、极线约束、角度投票（旋转一致性）进行剔除误匹配.

1）对极点邻域剔除
该特征点距离极点太近，表明kp2对应的MapPoint距离pKF1相机太近，所以也要剔除

if(!bStereo1 && !bStereo2)                                                
{                                                                         
    const float distex = ex-kp2.pt.x;                                     
    const float distey = ey-kp2.pt.y;                                                                                                 
    if(distex*distex+distey*distey < 100*pKF2->mvScaleFactors[kp2.octave])
        continue;                                                         
}                                                                         

2）极线约束
使用对极约束，得到 $p_1^T{F}_{12}{p}_2=0$ ，那么 $p_1^T{F}_{12}$ 将构成kp1在pKF2上的极线，然后计算kp2到该极线的距离，就对二者是否能够匹配上进行初步筛选，代码如下：

// kp1来自mpCurrentKeyFrame，kp2来自共视图
bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
    // Epipolar line in second image l = x1'F12 = [a b c]
    // 求出kp1在pKF2上对应的极线
    const float a = kp1.pt.x*F12.at<float>(0,0)+ kp1.pt.y*F12.at<float>(1,0)+ F12.at<float>(2,0);
    const float b = kp1.pt.x*F12.at<float>(0,1)+ kp1.pt.y*F12.at<float>(1,1)+ F12.at<float>(2,1);
    const float c = kp1.pt.x*F12.at<float>(0,2)+ kp1.pt.y*F12.at<float>(1,2)+ F12.at<float>(2,2);

    // 计算kp2特征点到极线的距离：
    // 极线l：ax + by + c = 0
    // (u,v)到l的距离为： |au+bv+c| / sqrt(a^2+b^2)

    const float num = a*kp2.pt.x+b*kp2.pt.y+c;

    const float den = a*a+b*b;

    if(den==0)
        return false;

    const float dsqr = num*num/den;

    /** @attention 尺度越大，范围应该越大!*/
    /** @attention 金字塔最底层一个像素就占一个像素，在倒数第二层，一个像素等于最底层1.2个像素（假设金字塔尺度为1.2）*/
    return dsqr < 3.84*pKF2->mvLevelSigma2[kp2.octave]; //阈值要根据尺度调整
}

2.6 SearchInNeighbors() 中的 Fuse()

这一步跟上面说到的SearchLocalPoints() 中的 SearchByProjection() 基本一样，会依次检查MapPoint在关键帧上投影的像素范围、深度范围、夹角小于60度、层检查、距离检查（chi2小于5.99）、描述子检查，最后找到该MapPoint在该区域最佳匹配的特征点，如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的），如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint.

---

最后，一图以蔽之

---

<完>
@leatherwang

---