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

过千帆er 05-05 87

前言：

目前大家对“特征点匹配跟踪算法”可能比较重视，你们都想要剖析一些“特征点匹配跟踪算法”的相关文章。那么小编也在网摘上汇集了一些有关“特征点匹配跟踪算法””的相关知识，希望各位老铁们能喜欢，你们一起来学习一下吧！

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

1、 SearchByBoW

函数SearchByBoW（）。通过词袋，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配

该函数中利用了词袋对特征点进行了匹配，先比较描述子之间的汉明距离来筛选一些较为合格的特征点对，然后再通过构建旋转直方图来筛选匹配质量较好的特征点。

先把整个函数挂在下方，然后分别通过图讲解其实现原理。

int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12){        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);        vector<int> rotHist[HISTO_LENGTH];    for(int i=0;i<HISTO_LENGTH;i++)        rotHist[i].reserve(500);        const float factor = HISTO_LENGTH/360.0f;    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();    while(f1it != f1end && f2it != f2end)    {                if(f1it->first == f2it->first)        {                        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;                                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;                    }                }                                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);        }    }        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;}123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146

2、图解每一步实现原理2.1 通过词袋模型寻找匹配

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

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

下面看代码：

1 准备工作：把两帧的特征点信息获取出来，并构建旋转直方图。

构建直方图的代码就几行，构建出来的直方图如图所示。

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

        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);         vector<int> rotHist[HISTO_LENGTH];    for(int i=0;i<HISTO_LENGTH;i++)        rotHist[i].reserve(500);        const float factor = HISTO_LENGTH/360.0f;    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();123456789101112131415161718192021222324252627

2 进行“替身”的搜索匹配。

FeatureVector主要用于不同图像特征点快速匹配，加速几何关系验证，

std::map<NodeId, std::vector > std::vector 中实际存的是NodeId 下所有特征点在图像中的索引。

f1it和f2it是词袋树中蓝色框图内的内容。

这样就可以看懂代码了，就是两层for循环就能得到两个“替身”的匹配，不过前提是这两个“替身”得在同一个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)    {                if(f1it->first == f2it->first)        {                        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;                                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;                    }                }                                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);        }    }1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192

3、添加旋转直方图信息

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

 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);  }1234567891011

图解：

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

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

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

 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)            {                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)                {                    CurrentFrame.mvpMapPoints[rotHist[i][j]]=NULL;                    nmatches--;                }            }        }    }1234567891011121314151617181920

以下是统计直方图中包含特征点信息较大的前三个bin。

void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3){    int max1=0;     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)     {        ind2=-1;        ind3=-1;    }    else if(max3<0.1f*(float)max1)    {        ind3=-1;    }}12345678910111213141516171819202122232425262728293031323334353637383940414243

3、期待您加入

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

文章知识点与官方知识档案匹配，可进一步学习相关知识

本文地址：http://www.longkongtuishu.com/ca381BA1sDFYGAV0.html

标签： #特征点匹配跟踪算法