前面,我们已经了解到了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++)
                {
                    const size_t idx2 = f2it->second[i2];

                    MapPoint* pMP2 = vpMapPoints2[idx2];

                    // 如果已经有匹配的点,或者遍历到的特征点对应的地图点无效
                    if(vbMatched2[idx2] || !pMP2)
                        continue;

                    if(pMP2->isBad())
                        continue;

                    const cv::Mat &d2 = Descriptors2.row(idx2);

                    int dist = DescriptorDistance(d1,d2);

                    if(dist<bestDist1)
                    {
                        bestDist2=bestDist1;
                        bestDist1=dist;
                        bestIdx2=idx2;
                    }
                    else if(dist<bestDist2)
                    {
                        bestDist2=dist;
                    }
                }

                // Step 5 对匹配结果进行检查,满足阈值、最优/次优比例,记录旋转直方图信息
                if(bestDist1<TH_LOW)
                {
                    if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
                    {
                        vpMatches12[idx1]=vpMapPoints2[bestIdx2];
                        vbMatched2[bestIdx2]=true;

                        if(mbCheckOrientation)
                        {
                            float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;
                            if(rot<0.0)
                                rot+=360.0f;
                            int bin = round(rot*factor);
                            if(bin==HISTO_LENGTH)
                                bin=0;
                            assert(bin>=0 && bin<HISTO_LENGTH);
                            rotHist[bin].push_back(idx1);
                        }
                        nmatches++;
                    }
                }
            }

            f1it++;
            f2it++;
        }
        else if(f1it->first < f2it->first)
        {
            f1it = vFeatVec1.lower_bound(f2it->first);
        }
        else
        {
            f2it = vFeatVec2.lower_bound(f1it->first);
        }
    }

    // Step 6 检查旋转直方图分布,剔除差异较大的匹配
    if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            if(i==ind1 || i==ind2 || i==ind3)
                continue;
            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
            {
                vpMatches12[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                nmatches--;
            }
        }
    }

    return nmatches;
}

2、图解每一步实现原理

2.1 通过词袋模型寻找匹配

在这里插入图片描述
上面是离线构建的词袋树,是提前通过聚类算法训练得到的。对实时采集的图像计算特征点,然后计算每个特征点的描述子,从离线创建好的vocabulary tree中开始找自己的位置,从根节点开始,用该描述子和每个节点的描述子计算汉明距离,选择汉明距离最小的作为自己所在的节点,一直遍历到叶子节点。也就是说,实时提取的特征点都在此词袋树的叶子节点找到了最相似的word作为自己的代替。从这个角度来说,其实用词袋模型来加速匹配,其实就是真实特征点在词袋树类里面“替身”与“替身”之间的匹配,这也是通过词袋模型来匹配会产生漏匹配的原因。

注意:最后给每个特征点寻找的“替身”节点包含两个信息: Word id 和weight

下面看代码:
1 准备工作:把两帧的特征点信息获取出来,并构建旋转直方图。
构建直方图的代码就几行,构建出来的直方图如图所示。

    // 构建旋转直方图,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

在这里插入图片描述

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

2 进行“替身”的搜索匹配。
FeatureVector主要用于不同图像特征点快速匹配,加速几何关系验证,
std::map<NodeId, std::vector > std::vector 中实际存的是NodeId 下所有特征点在图像中的索引。
f 1 i t 和 f 2 i t f1it和f2it f1it和f2it是词袋树中蓝色框图内的内容。
f1it
这样就可以看懂代码了,就是两层for循环就能得到两个“替身”的匹配,不过前提是这两个“替身”得在同一个 f i t fit fit中。
匹配找到后通过计算描述子的最优和次优距离筛除掉一些离群点,并记录旋转直方图信息。

    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++)
                {
                    const size_t idx2 = f2it->second[i2];

                    MapPoint* pMP2 = vpMapPoints2[idx2];

                    // 如果已经有匹配的点,或者遍历到的特征点对应的地图点无效
                    if(vbMatched2[idx2] || !pMP2)
                        continue;

                    if(pMP2->isBad())
                        continue;

                    const cv::Mat &d2 = Descriptors2.row(idx2);

                    int dist = DescriptorDistance(d1,d2);

                    if(dist<bestDist1)
                    {
                        bestDist2=bestDist1;
                        bestDist1=dist;
                        bestIdx2=idx2;
                    }
                    else if(dist<bestDist2)
                    {
                        bestDist2=dist;
                    }
                }

                // Step 5 对匹配结果进行检查,满足阈值、最优/次优比例,记录旋转直方图信息
                if(bestDist1<TH_LOW)
                {
                    if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
                    {
                        vpMatches12[idx1]=vpMapPoints2[bestIdx2];
                        vbMatched2[bestIdx2]=true;
						// 看第3步
                        if(mbCheckOrientation)
                        {
                            float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;
                            if(rot<0.0)
                                rot+=360.0f;
                            int bin = round(rot*factor);
                            if(bin==HISTO_LENGTH)
                                bin=0;
                            assert(bin>=0 && bin<HISTO_LENGTH);
                            rotHist[bin].push_back(idx1);
                        }
                        nmatches++;
                    }
                }
            }

            f1it++;
            f2it++;
        }
        else if(f1it->first < f2it->first)
        {
            f1it = vFeatVec1.lower_bound(f2it->first);
        }
        else
        {
            f2it = vFeatVec2.lower_bound(f1it->first);
        }
    }

3、添加旋转直方图信息
提取特征点时,由于ORB特征点是带有旋转信息的,所以通过计算匹配的两个特征点旋转角度的差异构建旋转直方图。代码如下。

 if(mbCheckOrientation)
 {
     float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;
     if(rot<0.0)
     	rot+=360.0f;
     int bin = round(rot*factor);//factor是 1/12
     if(bin==HISTO_LENGTH)
     	bin=0;
     assert(bin>=0 && bin<HISTO_LENGTH);
     rotHist[bin].push_back(idx1);
  }

图解:
在这里插入图片描述

2.2 通过旋转直方图来筛除离群点

上一步把所有的特征点对的旋转角度差信息都添加到了旋转直方图中去了,然后对旋转直方图中特征点的分布,剔除差异较大的匹配。

思想是:选出idx1落入最多的3个bin,这些点表示较多的点发生了一致性旋转代表正确匹配,保留。其他点置为NULL。

 if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;
		//筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引,具体实现看后面。
        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            if(i!=ind1 && i!=ind2 && i!=ind3)
            {
                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
                {
                    CurrentFrame.mvpMapPoints[rotHist[i][j]]=NULL;//没有旋转一致性的点抹去。
                    nmatches--;
                }
            }
        }
    }

在这里插入图片描述
以下是统计直方图中包含特征点信息较大的前三个bin。

void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)//引用 所以这个就不是形参了,是实参
{
    int max1=0; //定义了3个变量
    int max2=0;
    int max3=0;

    for(int i=0; i<L; i++)//循环直方图
    {
        const int s = histo[i].size(); //取出当前循环的直方图大小
        if(s>max1)
        {
            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) //  max2/max1小于0.1,就把max2 max3都不要了,  如max1=200  max2=15  max=12 ,都集中在1里, 2 3不要了
    {
        ind2=-1;
        ind3=-1;
    }
    else if(max3<0.1f*(float)max1)//max3/max1小于0.1,就只不要max3了,如 max1=200  max2=150  max3=15
    {
        ind3=-1;
    }
}

3、 期待您加入

也非常期待您能关注我的微信公众号–“过千帆”,里面不仅有技术文章还有我的读书分享,希望您在那里也有收获。我们一起进步。

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐