(01)ORB-SLAM2源码无死角解析-(32) ORB特征匹配→跟踪线程BoW加速匹配,关键帧特征点跟踪SearchByBoW()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/123092196
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX官方认证
 

一、前言

前面两篇博客对 BoW 的构建,以及:BRIEF描述子转BoW向量进行了讲解。那么他有什么作用呢? 或者说,在什么地方可以得到应用。其实,利用到他的地方很多,比如跟踪线程中得关键帧特征点跟踪SearchByBoW()、局部建图线程中得搜索匹配三角化SearchForTriangulation。本来这个是后面进行讲解得内容,但是为了让大家了解源码是如何通过BoW加速匹配得。这里提前为大家分析这两个案例。这篇博客,就先来看看SearchByBoW()函数

 

二、源码流程

需要讲解的函数为: src/ORBmatcher.cc 文件中的 ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)。该函数是为参考帧与普通帧之间的特征点匹配,还有另外一个重载函数,为关键帧与关键帧之间的特征匹配。两个重载函数的原理基本一致的,所以只讲解一个。

首先,需要明白该函数的目的: 输入一张关键帧 pKF,以及一张普通帧 F,然后获得匹配特征点对应的地图点vpMapPointMatches。

( 1 ) : \color{blue}{(1)}: (1) 获取关键帧的地图点(关键帧能够观测到的所有地图点)vpMapPointsKF;以及关键帧的词袋特征向量 vFeatVecKF,其存储多个元素,每个元素包含两个值,第一个值是所属节点id,以及该节点下所有特征点在图像中的索引。

( 2 ) : \color{blue}{(2)}: (2)构建角度差直方图,这个内容在前面的博客中已经进行过讲解 rotHist,主要是去除一些不符合主流方向的特征点,或者说异常点,也就是所谓的外点。

( 3 ) : \color{blue}{(3)}: (3)循环对关键帧 pKF,普通帧 F的特征所属节点,如果两个所属节点 id 相同。遍历该所属节点 pKF 的所有特征点,如果该特征点存在对应的地图点且不是坏点,则与该所属节点普通帧 F 的所有特征点进行匹配,即计算BRIEF描述子之间的汉明距离。

( 4 ) : \color{blue}{(4)}: (4)计算汉明距离的时候,会记录下最优距离以及次优距离,只有满足: ①最优距离小于指定阈值,②最佳匹配比次佳匹配明显要好。则两个条件,才认为是成功匹配。

( 5 ) : \color{blue}{(5)}: (5)成功匹配之后,把关键帧中与之对应的地图点赋值给 vpMapPointMatches。完成所有特征点点匹配之后,再通过角度差直方图,去除不符合主流方向的地图点,也就是设置为 NULL。返回成功匹配的地图点数目。

另外这里额外提及一些细节的东西: SearchByBoW 函数中存在代码如下:

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

因为 vFeatVec1,与 vFeatVec2 都是按照所属节点 id 从小到大排序的。如果 pKF1 中所属节点与 pKF2 中所属节点不匹配,则有两种情况:
①.f1it->first(pKF1所属节点id) 小于 f2it->first(pKF2所属节点id) →那么就把 f1it 的节点 id 增大,赋值成与 f2it 相同的节点,那么下一次循环,他们所属节点 id 就相同了。
②.f1it->first(pKF1所属节点id) 大于 f2it->first(pKF2所属节点id) →那么就把 f2it 的节点 增大,赋值成与 f1it 相同的节点,那么下一次循环,他们所属节点 id 就相同了。

 

三、源码注释

/*
 * @brief 通过词袋,对关键帧的特征点进行跟踪
 * 步骤
 * Step 1:分别取出属于同一node的ORB特征点(只有属于同一node,才有可能是匹配点)
 * Step 2:遍历KF中属于该node的特征点
 * Step 3:遍历F中属于该node的特征点,寻找最佳匹配点
 * Step 4:根据阈值 和 角度投票剔除误匹配
 * Step 5:根据方向剔除误匹配的点
 * @param  pKF               关键帧
 * @param  F                 当前普通帧
 * @param  vpMapPointMatches F中地图点对应的匹配,NULL表示未匹配
 * @return                   成功匹配的数量
 */
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
    // 获取该关键帧的地图点
    const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();

    // 和普通帧F特征点的索引一致
    vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));

    // 取出关键帧的词袋特征向量
    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);

    // 将0~360的数转换到0~HISTO_LENGTH的系数
    // !原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的,更改为下面代码  
    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)
    {
        // Step 1:分别取出属于同一node的ORB特征点(只有属于同一node,才有可能是匹配点)
        // first 元素就是node id,遍历
        if(KFit->first == Fit->first) 
        {
            // second 是该node内存储的feature index
            const vector<unsigned int> vIndicesKF = KFit->second;
            const vector<unsigned int> vIndicesF = Fit->second;

            // Step 2:遍历KF中属于该node的特征点
            for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
            {
                // 关键帧该节点中特征点的索引
                const unsigned int realIdxKF = vIndicesKF[iKF];

                // 取出KF中该特征对应的地图点
                MapPoint* pMP = vpMapPointsKF[realIdxKF]; 

                if(!pMP)
                    continue;

                if(pMP->isBad())
                    continue;

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

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

                // Step 3:遍历F中属于该node的特征点,寻找最佳匹配点
                for(size_t iF=0; iF<vIndicesF.size(); iF++)
                {
                    // 和上面for循环重名了,这里的realIdxF是指普通帧该节点中特征点的索引
                    const unsigned int realIdxF = vIndicesF[iF];

                    // 如果地图点存在,说明这个点已经被匹配过了,不再匹配,加快速度
                    if(vpMapPointMatches[realIdxF])
                        continue;

                    const cv::Mat &dF = F.mDescriptors.row(realIdxF); // 取出F中该特征对应的描述子
                    // 计算描述子的距离
                    const int dist =  DescriptorDistance(dKF,dF); 

                    // 遍历,记录最佳距离、最佳距离对应的索引、次佳距离等
                    // 如果 dist < bestDist1 < bestDist2,更新bestDist1 bestDist2
                    if(dist<bestDist1)
                    {
                        bestDist2=bestDist1;
                        bestDist1=dist;
                        bestIdxF=realIdxF;
                    }
                    // 如果bestDist1 < dist < bestDist2,更新bestDist2
                    else if(dist<bestDist2) 
                    {
                        bestDist2=dist;
                    }
                }

                // Step 4:根据阈值 和 角度投票剔除误匹配
                // Step 4.1:第一关筛选:匹配距离必须小于设定阈值
                if(bestDist1<=TH_LOW) 
                {
                    // Step 4.2:第二关筛选:最佳匹配比次佳匹配明显要好,那么最佳匹配才真正靠谱
                    if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
                    {
                        // Step 4.3:记录成功匹配特征点的对应的地图点(来自关键帧)
                        vpMapPointMatches[bestIdxF]=pMP;

                        // 这里的realIdxKF是当前遍历到的关键帧的特征点id
                        const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];

                        // Step 4.4:计算匹配点旋转角度差所在的直方图
                        if(mbCheckOrientation)
                        {
                            // 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);       // 直方图统计
                        }
                        nmatches++;
                    }
                }

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

    // Step 5 根据方向剔除误匹配的点
    if(mbCheckOrientation)
    {
        // index
        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)
                continue;

            // 剔除掉不在前三的匹配对,因为他们不符合“主流旋转方向”  
            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
            {
                vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                nmatches--;
            }
        }
    }

    return nmatches;
}

 

四、结语

通过这篇博客,大家知道了为什么使用 BoW 可以可以加快特征匹配,其实他的原理很简单,那就是两帧图像在进行特征对比的时候,只有所属节点id下的特征点才进行两两比较。下一篇博客,还会对局部建图BoW加速匹配,中的三角化SearchForTriangulation()函数进行讲解。

 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件

  • 1
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值