(01)ORB-SLAM2源码无死角解析-(33) ORB特征匹配→局部建图BoW加速匹配，三角化SearchForTriangulation

江南才尽，年少无知！

已于 2022-10-31 17:30:50 修改

阅读量1.1k

点赞数 2

分类专栏： (01)ORB-SLAM2源码无死角解析-收费文章标签：无人机 ORB-SLAM2 增强现实自动驾驶机器人

于 2022-05-24 12:51:02 首次发布

原文链接：https://cvlife.net/detail/term_634260915e015_SerHfL/25?product_id=term_634260915e015_SerHfL

版权

(01)ORB-SLAM2源码无死角解析-收费专栏收录该内容

15 篇文章 10 订阅 ¥69.90 ¥99.00

订阅专栏

超级会员免费看

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

前面两篇博客对 BoW 的构建，以及:BRIEF描述子转BoW向量进行了讲解。那么他有什么作用呢? 或者说，在什么地方可以得到应用。其实，利用到他的地方很多，比如局部建图线程搜索匹配三角化SearchForTriangulation。SearchForTriangulation 函数是在局部建图中被调用的，本来是后面才讲解的内容，但是为了更深一步了解 BoW 的应用，所以提前对其进行一个讲解。

该代码位于 src/ORBmatcher.cc 中，函数名为int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,ector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)。该函数的主要作用是: 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对。下面来做一个具体的分析。另外这里为附上一个参考图:
在这里插入图片描述

二、源码流程

$\color{blue}{(1)}：$ 计算KF1的相机中心在KF2图像平面的二维像素坐标，这个像素坐标也叫极点，对应于上图的 $e_1$ 。创建变量 vbMatched2，vMatches12用来记录是否匹配成功，避免重复匹配。rotHist 用来统计旋转差直方图。

$\color{blue}{(2)}：$ 利用BoW加速匹配: 只对属于同一节点(特定层)的ORB特征进行匹配。vFeatVec1，vFeatVec2 每个元素的第一个值，存储的是所属节点 id；第二值存储的是特征点或者BRIEF描述子的索引。vFeatVec1，vFeatVec2 中的元素按照所属 id 从小到大进行排列。

$\color{blue}{(3)}：$ 遍历pKF1和pKF2所有所属节点，如果所属节点相同，则对两个节点下的特征点进行匹配。

$\color{blue}{(4)}：$ 遍历pKF1和pKF2同一所属节点下的所有特征点，进行两两匹配，如果特征点已经匹配过了，或者存在与之对应的地图点，则跳过该特征点，不进行匹配。

$\color{blue}{(5)}：$ pKF2 上的特征点距离极点太近，则认为该离摄像头太近了，则跳过不进行特征匹配。

$\color{blue}{(6)}：$ kp2(当前帧特征点)应该位于kp1对应的极线上，也就是说，上图中的 $P_1$ 特征点应该距离极线 $l_1$ 最近(后面有细节分析)。如果距离过远，认为匹配失败。同时记录最近的特征点，认为是最佳匹配点。

$\color{blue}{(7)}：$ 记录旋转差直方图信息，该内容在前面的博客中讲解过，主要是记录主流特征点，删除异常匹配点特征点。

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

        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 就相同了。

判断关键点与极线距离函数为CheckDistEpipolarLine(kp1,kp2,F12,pKF2)，通过前面博客，根据对极几何原理，可得基本矩阵相关公式: $\tag{01} \color{green} \mathbf F_{ab}=(\mathbf K_b^{-T} \mathbf E_{ab}\mathbf K_a^{-1}) ~~~~~~~~~~~~~~~~~ \color{green} v_a^T\mathbf F_{ab}v_b=0$
换成源码中的公式为(相机相同，故 $\mathbf K_1=\mathbf K_2$ ):
$\tag{02} \color{green} \mathbf F_{12}=(\mathbf K^{-T} \mathbf E_{12}\mathbf K) ~~~~~~~~~~~~~~~~~~~~ \color{green} v_1^T\mathbf F_{12}v_2=0$ 如果把 $v_1^T\mathbf F_{12}$ 看作极线，则其表示为在帧 pKF2 的极线，如果把 $\mathbf F_{12}v_2$ 看作极线，则表示在帧pKF1中的极线，则表示在帧 pKF1 的极线。源码中是把 $v_1^T\mathbf F_{12}$ 看作极线，然后再计算 pKF2 中的特征点 kp2 距离极线的距离是否在允许偏差范围内，如果直线(极线)方程为 $a x + b y + c = 0$ , 那么点到直线的距离为:
$\tag{02} \color{green} d = \frac{|ax+bx+c|}{\sqrt{a^2+b^2}}$

四、源码注释

该该代码位于 src/ORBmatcher.cc 中，函数名为 ORBmatcher::SearchForTriangulation()。

/**
 * @brief 用基础矩阵检查极线距离是否符合要求
 * 
 * @param[in] kp1               KF1中特征点
 * @param[in] kp2               KF2中特征点  
 * @param[in] F12               从KF2到KF1的基础矩阵
 * @param[in] pKF2              关键帧KF2
 * @return true 
 * @return false 
 */
bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
    // Epipolar line in second image l2 = x1'F12 = [a b c]
    // Step 1 求出kp1在pKF2上对应的极线
    const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
    const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
    const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);

    // Step 2 计算kp2特征点到极线l2的距离
    // 极线l2：ax + by + c = 0
    // (u,v)到l2的距离为： |au+bv+c| / sqrt(a^2+b^2)

    const float num = a*kp2.pt.x+b*kp2.pt.y+c;

    const float den = a*a+b*b;

    // 距离无穷大
    if(den==0)
        return false;

    // 距离的平方
    const float dsqr = num*num/den;

    // Step 3 判断误差是否满足条件。尺度越大，误差范围应该越大。
    // 金字塔最底层一个像素就占一个像素，在倒数第二层，一个像素等于最底层1.2个像素（假设金字塔尺度为1.2）
    // 3.84 是自由度为1时，服从高斯分布的一个平方项（也就是这里的误差）小于一个像素，这件事发生概率超过95%时的概率 （卡方分布）
    return dsqr < 3.84*pKF2->mvLevelSigma2[kp2.octave];
}


/*
 * @brief 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对
 * 具体来说，pKF1图像的每个特征点与pKF2图像同一node节点的所有特征点依次匹配，判断是否满足对极几何约束，满足约束就是匹配的特征点
 * @param pKF1          关键帧1
 * @param pKF2          关键帧2
 * @param F12           从2到1的基础矩阵
 * @param vMatchedPairs 存储匹配特征点对，特征点用其在关键帧中的索引表示
 * @param bOnlyStereo   在双目和rgbd情况下，是否要求特征点在右图存在匹配
 * @return              成功匹配的数量
 */
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)
{
    const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;

    // Compute epipole in second image
    // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
    // KF1相机光心在世界坐标系坐标Cw
    cv::Mat Cw = pKF1->GetCameraCenter(); 
    // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
    cv::Mat R2w = pKF2->GetRotation();    
    cv::Mat t2w = pKF2->GetTranslation(); 
    // KF1的相机光心转化到KF2坐标系中的坐标
    cv::Mat C2 = R2w*Cw+t2w; 
    const float invz = 1.0f/C2.at<float>(2);
    // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
    const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
    const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;

    // Find matches between not tracked keypoints
    // Matching speed-up by ORB Vocabulary
    // Compare only ORB that share the same node

    int nmatches=0;
    // 记录匹配是否成功，避免重复匹配
    vector<bool> vbMatched2(pKF2->N,false);        
    vector<int> vMatches12(pKF1->N,-1);
    // 用于统计匹配点对旋转差的直方图
    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;

    // We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
    // Step 2 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配
    // FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历
    // FeatureVector的数据结构类似于：{(node1,feature_vector1) (node2,feature_vector2)...}
    // f1it->first对应node编号，f1it->second对应属于该node的所有特特征点编号
    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();
    
    // Step 2.1：遍历pKF1和pKF2中的node节点
    while(f1it!=f1end && f2it!=f2end)
    {
        // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
        if(f1it->first == f2it->first)
        {
            // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
            for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
            {
                // 获取pKF1中属于该node节点的所有特征点索引
                const size_t idx1 = f1it->second[i1];
                
                // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
                MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
                
                // If there is already a MapPoint skip
                // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
                if(pMP1)
                    continue;

                // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
                const bool bStereo1 = pKF1->mvuRight[idx1]>=0;

                if(bOnlyStereo)
                    if(!bStereo1)
                        continue;
                
                // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
                const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
                
                // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
                const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
                
                int bestDist = TH_LOW;
                int bestIdx2 = -1;
                
                // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
                for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
                {
                    // 获取pKF2中属于该node节点的所有特征点索引
                    size_t idx2 = f2it->second[i2];
                    
                    // 通过特征点索引idx2在pKF2中取出对应的MapPoint
                    MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
                    
                    // If we have already matched or there is a MapPoint skip
                    // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
                    if(vbMatched2[idx2] || pMP2)
                        continue;

                    const bool bStereo2 = pKF2->mvuRight[idx2]>=0;

                    if(bOnlyStereo)
                        if(!bStereo2)
                            continue;
                    
                    // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
                    const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
                    
                    // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
                    const int dist = DescriptorDistance(d1,d2);
                    
                    if(dist>TH_LOW || dist>bestDist)
                        continue;

                    // 通过特征点索引idx2在pKF2中取出对应的特征点
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];

                    //? 为什么双目就不需要判断像素点到极点的距离的判断？
                    // 因为双目模式下可以左右互匹配恢复三维点
                    if(!bStereo1 && !bStereo2)
                    {
                        const float distex = ex-kp2.pt.x;
                        const float distey = ey-kp2.pt.y;
                        // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
                        // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
                        // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
                        //? 对这个阈值的有效性持怀疑态度
                        if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
                            continue;
                    }

                    // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
                    if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
                    {
                        // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
                        bestIdx2 = idx2;
                        bestDist = dist;
                    }
                }

                
                if(bestIdx2>=0)
                {
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
                    // 记录匹配结果
                    vMatches12[idx1]=bestIdx2;      
                    
                    vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
                    nmatches++;

                    // 记录旋转差直方图信息
                    if(mbCheckOrientation)
                    {
                        // angle：角度，表示匹配点对的方向差。
                        float rot = kp1.angle-kp2.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);
                    }
                }
            }

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

    // Step 3 用旋转差直方图来筛掉错误匹配对
    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++)
            {              
                vbMatched2[vMatches12[rotHist[i][j]]] = false;  // !清除匹配关系。原作者漏掉！
                vMatches12[rotHist[i][j]]=-1;
                nmatches--;
            }
        }

    }

    // Step 4 存储匹配关系，下标是关键帧1的特征点id，存储的是关键帧2的特征点id
    vMatchedPairs.clear();
    vMatchedPairs.reserve(nmatches);

    for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
    {
        if(vMatches12[i]<0)
            continue;
        vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
    }

    return nmatches;
}

五、结语

通过这篇博客，讲解了 BoW 的应用，可以很明显的知道加速匹配的原理， SearchForTriangulation 函数是在局部建图中被调用的，后续我们会对齐进行详细的讲解。

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

江南才尽，年少无知！

关注

2
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
(01)ORB-SLAM2源码无死角解析-(33) ORB特征匹配→局部建图BoW加速匹配，三角化SearchForTriangulation

前面两篇博客对 BoW 的构建，以及:BRIEF描述子转BoW向量进行了讲解。那么他有什么作用呢? 或者说，在什么地方可以得到应用。其实，利用到他的地方很多，比如局部建图线程搜索匹配三角化SearchForTriangulation。SearchForTriangulation 函数是在局部建图中被调用的，本来是后面才讲解的内容，但是为了更深一步了解 BoW 的应用，所以提前对其进行一个讲解。......
复制链接

扫一扫

专栏目录