4、ORB-SLAM闭环检测之通过词袋寻找当前关键帧和闭环候选帧之间的匹配

前面，我们已经了解到了sim3的求解流程，具体计算过程中有三步比较重要： 1、寻找两关键帧之间的粗匹配来粗略估计sim3；2、由粗略估计出的sim3来寻找两关键帧之间更多的匹配；3、构建图优化模型，固定地图点通过两关键帧的地图点到两帧的重投影误差来优化sim3.

1、 SearchByBoW

函数SearchByBoW（）。通过词袋，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配
该函数中利用了词袋对特征点进行了匹配，先比较描述子之间的汉明距离来筛选一些较为合格的特征点对，然后再通过构建旋转直方图来筛选匹配质量较好的特征点。
先把整个函数挂在下方，然后分别通过图讲解其实现原理。

int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)
{
   
    // Step 1 分别取出两个关键帧的特征点、BoW 向量、地图点、描述子
    const vector<cv::KeyPoint> &vKeysUn1 = pKF1->mvKeysUn;
    const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
    const cv::Mat &Descriptors1 = pKF1->mDescriptors;

    const vector<cv::KeyPoint> &vKeysUn2 = pKF2->mvKeysUn;
    const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
    const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
    const cv::Mat &Descriptors2 = pKF2->mDescriptors;

    // 保存匹配结果
    vpMatches12 = vector<MapPoint*>(vpMapPoints1.size(),static_cast<MapPoint*>(NULL));
    vector<bool> vbMatched2(vpMapPoints2.size(),false);//闭环候选关键帧的地图点匹配结果

    // Step 2 构建旋转直方图，HISTO_LENGTH = 30 
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);

    //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
    const float factor = HISTO_LENGTH/360.0f;// 1/12

    int nmatches = 0;

	//FeatureVector主要用于不同图像特征点快速匹配，加速几何关系验证，
	//std::map<NodeId, std::vector<unsigned int> >  std::vector 中实际存的是NodeId 下所有特征点在图像中的索引。
    DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
    DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
    DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
    DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();

    while(f1it != f1end && f2it != f2end)
    {
   
        // Step 3 开始遍历，分别取出属于同一node的特征点(只有属于同一node，才有可能是匹配点)
        if(f1it->first == f2it->first)
        {
   
            // 遍历KF中属于该node的特征点
            for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
            {
   
                const size_t idx1 = f1it->second[i1];//属于该节点下的所有特征点索引

                MapPoint* pMP1 = vpMapPoints1[idx1];
                if(!pMP1)
                    continue;
                if(pMP1->isBad())
                    continue;

                const cv::Mat &d1 = Descriptors1.row(idx1);

                int bestDist1=256;
                int bestIdx2 =-1 ;
                int bestDist2=256;

                // Step 4 遍历KF2中属于该node的特征点，找到了最优及次优匹配点
                for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
                {