📝笔记：SLAM常见问题(一)：SearchByBoW

ORB-SLAM中使用了多种特征匹配的奇技淫巧，其中之一就是利用词袋信息进行引导匹配SearchByBoW：利用了BOW里的正向引导进行两帧之间的匹配，核心点在于位于同一个节点处的特征才有可能属于同一匹配，相较于暴力匹配匹配速度更快。

注意：每幅图像都可以通过ComputeBoW得到其对应的词袋向量。featureVector存储的是节点的索引值以及对应图像feature对应的索引向量，即map<node_id,vector<featureID>。这样的话就可以根据两帧图像的node_id来初步确定二者共有的特征点，然后根据该id取出vector<featureID>，根据featureID找到图像上的特征点以及描述子，通过比较二者描述子距离来判定该特征点是否为匹配点，若距离小于某一阈值，则二者为匹配对。

/**
 * @brief Bag of Words Representation
 * 计算词袋mBowVec和mFeatVec
 * @see CreateInitialMapMonocular() TrackReferenceKeyFrame() Relocalization()
 */
//同样的，关键帧也有构造函数void KeyFrame::ComputeBoW()
void Frame::ComputeBoW() 
{
    if(mBowVec.empty())
    {
        // 将描述子mDescriptors转换为DBOW要求的输入格式
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        // 转换成词袋向量mBowVec以及特征向量mFeatVec
        // mBowVec存储着单词及其对应的权重TF-IDF值
        // mFeatVec存储节点ID以及对应对应图像feature对应的索引向量
        mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
    }
}

该函数在Tracking线程中的TrackReferenceKeyFrame()/Relocalization()进行调用（注意：LoopClosing线程中ComputeSim3()也会调用该函数，与上述二者的区别在于，ComputeSim3()中的SearchByBoW是寻找关键帧之间的匹配，而非关键帧与当前帧之间的匹配）。

下面给出ORB-SLAM2中用于关键帧与当前帧进行词袋引导匹配的源码：

/**
 * @brief 通过词袋，对关键帧的特征点进行跟踪
 * 
 * 通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配） \n
 * 对属于同一node的特征点通过描述子距离进行匹配 \n
 * 根据匹配，用pKF中特征点对应的MapPoint去更新F中特征点对应的MapPoints \n
 * 每个特征点都对应一个MapPoint，因此pKF中每个特征点的MapPoint也就是F中对应点的MapPoint \n
 * 通过距离阈值、比例阈值和角度投票进行剔除误匹配
 * @param  pKF               KeyFrame
 * @param  F                 Current Frame
 * @param  vpMapPointMatches F中MapPoints对应的匹配，NULL表示未匹配
 * @return                   成功匹配的数量
 */
 //const int ORBmatcher::TH_HIGH = 100;
 //const int ORBmatcher::TH_LOW = 50;
 //const int ORBmatcher::HISTO_LENGTH = 30;
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
    // vpMapPointsKF：获取输入关键帧匹配到的地图点
    const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();

    // 初始化当前帧MapPoints对应的匹配NULL
    vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));

    // FeatureVector数据类型 map<node_id,vector<featureID>，
    // 可以快速根据node_id找到属于该node的特征点
    const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;

    int nmatches=0;

    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);
    const float factor = HISTO_LENGTH/360.0f;

    // We perform the matching over ORB that belong to the same vocabulary node 
    // (at a certain level)
    // 建立迭代器，将属于同一节点(特定层)的ORB特征进行匹配
    DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
    DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
    DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
    DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();

    while(KFit != KFend && Fit != Fend)
    {
        // 步骤1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
        // first表示node_id，只有node_id相同才表示这些特征点位于同一层
        if(KFit->first == Fit->first) 
        {
            // second中记录了这些特征对应图像中的ID
            const vector<unsigned int> vIndicesKF = KFit->second;
            const vector<unsigned int> vIndicesF = Fit->second;

            // 步骤2：遍历KF中属于该node的特征点
            for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
            {
                // 获取关键帧上某一个特征点的ID
                const unsigned int realIdxKF = vIndicesKF[iKF];
                // 根据该ID得到该特征对应的MapPoint
                MapPoint* pMP = vpMapPointsKF[realIdxKF]; 

                if(!pMP) //不存在
                    continue;

                if(pMP->isBad())//被标记为坏点
                    continue;

                // 根据该ID取出KF中该特征对应的描述子
                const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); 

                int bestDist1=256; // 最好的距离（最小距离）
                int bestIdxF =-1 ;
                int bestDist2=256; // 倒数第二好距离（倒数第二小距离）

                // 步骤3：遍历当前帧中属于该node的特征点，找到了最佳匹配点
                for(size_t iF=0; iF<vIndicesF.size(); iF++)
                {
                    // 取出当前帧上位于该node上的某一个特征点的ID
                    const unsigned int realIdxF = vIndicesF[iF];
                    // 表明这个点已经被匹配过了，不再匹配，加快速度
                    if(vpMapPointMatches[realIdxF])
                        continue;
                    // 取出F中该特征对应的描述子
                    const cv::Mat &dF = F.mDescriptors.row(realIdxF); 

                    // 计算描述子距离，这里是汉明距离，若非二进制描述子可选择用其他距离
                    const int dist =  DescriptorDistance(dKF,dF); 

                    // 下面的操作就是分别获得最小bestDist1以及次小bestDist2的描述子距离
                    // dist < bestDist1 < bestDist2，更新bestDist1 bestDist2
                    if(dist<bestDist1)
                    {
                        bestDist2=bestDist1;
                        bestDist1=dist;
                        bestIdxF=realIdxF;
                    }
                    else if(dist<bestDist2)// bestDist1 < dist < bestDist2，更新bestDist2
                    {
                        bestDist2=dist;
                    }
                }

                // 步骤4：根据描述子距离阈值和角度投票剔除误匹配
                // 最小的描述子距离小于一个阈值 
                if(bestDist1<=TH_LOW) 
                {
                    // trick!
                    // 最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱
                    if(static_cast<float>(bestDist1)<
                    mfNNratio*static_cast<float>(bestDist2))
                    {
                        // 步骤5：更新当前帧特征点的MapPoint
                        // 记录了特征点ID以及对应的特征点
                        vpMapPointMatches[bestIdxF]=pMP;

                        // 获得关键帧上的特征点位置
                        const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];

                        //
                        if(mbCheckOrientation)
                        {
                            // trick!
                            // angle：每个特征点在提取描述子时的旋转主方向角度，
                            // 如果图像旋转了，这个角度将发生改变
                            // 所有的特征点的角度变化应该是一致的，
                            // 通过直方图统计得到最准确的角度变化值
                            // 该特征点的角度变化值
                            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);
                        }
                        // 匹配点+1
                        nmatches++;
                    }
                }

            }

            KFit++;
            Fit++;
        }
        else if(KFit->first < Fit->first)
        {
            KFit = vFeatVecKF.lower_bound(Fit->first);
        }
        else
        {
            Fit = F.mFeatVec.lower_bound(KFit->first);
        }
    }

    // 根据方向剔除误匹配的点，即删除那些不属于特征点角度变化最多的三个类别的匹配点
    if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        // 计算rotHist中最大的三个的index
        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            // 如果特征点的旋转角度变化量属于这三个组，则保留
            if(i==ind1 || i==ind2 || i==ind3)
                continue;

            // 将除了ind1 ind2 ind3以外的匹配点去掉
            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
            {
                vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                nmatches--;
            }
        }
    }

    return nmatches;
}

另外，LoopClosing线程中ComputeSim3()调用的SearchByBoW的函数声明为：

int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)

参考链接

1. https://blog.csdn.net/qq_24893115/article/details/52629248