• ORB-SLAM2 ---- ORBmatcher::SearchForTriangulation函数

    目录

    1.推荐先读博客

    2.源码解析 

    2.1 源代码

    2.2 关于基础矩阵 

    2.3  计算KF1的相机中心在KF2图像平面的二维像素坐标

    2.4 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配 

    1.推荐先读博客

            为了更好的理解本节内容，推荐看懂以下内容再看本节：

    ORB-SLAM2 ---- ORBmatcher::SearchByBoW函数_Courage2022的博客-CSDN博客icon-default.png?t=M666https://blog.csdn.net/qq_41694024/article/details/126322962ORB-SLAM2 ---- Frame::ComputeBoW函数_Courage2022的博客-CSDN博客icon-default.png?t=M666https://blog.csdn.net/qq_41694024/article/details/126328833

    2.源码解析 

    2.1 源代码

    1. /*
    2.  * @brief 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对
    3.  * 具体来说，pKF1图像的每个特征点与pKF2图像同一node节点的所有特征点依次匹配，判断是否满足对极几何约束，满足约束就是匹配的特征点
    4.  * @param pKF1          关键帧1
    5.  * @param pKF2          关键帧2
    6.  * @param F12           从2到1的基础矩阵
    7.  * @param vMatchedPairs 存储匹配特征点对，特征点用其在关键帧中的索引表示
    8.  * @param bOnlyStereo   在双目和rgbd情况下，是否要求特征点在右图存在匹配
    9.  * @return              成功匹配的数量
    10.  */
    11. int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
    12.                                        vectorsize_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)
    13. {
    14.     const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    15.     const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
    16.  
    17.     // Compute epipole in second image
    18.     // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
    19.     // KF1相机光心在世界坐标系坐标Cw
    20.     cv::Mat Cw = pKF1->GetCameraCenter(); 
    21.     // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
    22.     cv::Mat R2w = pKF2->GetRotation();    
    23.     cv::Mat t2w = pKF2->GetTranslation(); 
    24.     // KF1的相机光心转化到KF2坐标系中的坐标
    25.     cv::Mat C2 = R2w*Cw+t2w; 
    26.     const float invz = 1.0f/C2.at<float>(2);
    27.     // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
    28.     const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
    29.     const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;
    30.  
    31.     // Find matches between not tracked keypoints
    32.     // Matching speed-up by ORB Vocabulary
    33.     // Compare only ORB that share the same node
    34.  
    35.     int nmatches=0;
    36.     // 记录匹配是否成功，避免重复匹配
    37.     vector<bool> vbMatched2(pKF2->N,false);        
    38.     vector<int> vMatches12(pKF1->N,-1);
    39.     // 用于统计匹配点对旋转差的直方图
    40.     vector<int> rotHist[HISTO_LENGTH];
    41.     for(int i=0;i
    42.         rotHist[i].reserve(500);
    43.  
    44.      //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
    45.     const float factor = HISTO_LENGTH/360.0f;
    46.  
    47.     // We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
    48.     // Step 2 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配
    49.     // FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历
    50.     // FeatureVector的数据结构类似于：{(node1,feature_vector1) (node2,feature_vector2)...}
    51.     // f1it->first对应node编号，f1it->second对应属于该node的所有特特征点编号
    52.     DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
    53.     DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
    54.     DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
    55.     DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
    56.  
    57.     // Step 2.1：遍历pKF1和pKF2中的node节点
    58.     while(f1it!=f1end && f2it!=f2end)
    59.     {
    60.         // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
    61.         if(f1it->first == f2it->first)
    62.         {
    63.             // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
    64.             for(size_t i1=0, iend1=f1it->second.size(); i1
    65.             {
    66.                 // 获取帧1即pKF1中属于该node节点的描述子索引 0 --- 数量-1
    67.                 const size_t idx1 = f1it->second[i1];
    68.                 
    69.                 // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
    70.                 MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
    71.                 
    72.                 // If there is already a MapPoint skip
    73.                 // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
    74.                 if(pMP1)
    75.                     continue;
    76.  
    77.                 // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
    78.                 const bool bStereo1 = pKF1->mvuRight[idx1]>=0;
    79.  
    80.                 if(bOnlyStereo)
    81.                     if(!bStereo1)
    82.                         continue;
    83.                 
    84.                 // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
    85.                 const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
    86.                 
    87.                 // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
    88.                 const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
    89.                 
    90.                 int bestDist = TH_LOW;
    91.                 int bestIdx2 = -1;
    92.                 
    93.                 // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
    94.                 for(size_t i2=0, iend2=f2it->second.size(); i2
    95.                 {
    96.                     // 获取pKF2中属于该node节点的所有特征点索引
    97.                     size_t idx2 = f2it->second[i2];
    98.                     
    99.                     // 通过特征点索引idx2在pKF2中取出对应的MapPoint
    100.                     MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
    101.                     
    102.                     // If we have already matched or there is a MapPoint skip
    103.                     // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
    104.                     if(vbMatched2[idx2] || pMP2)
    105.                         continue;
    106.  
    107.                     const bool bStereo2 = pKF2->mvuRight[idx2]>=0;
    108.  
    109.                     if(bOnlyStereo)
    110.                         if(!bStereo2)
    111.                             continue;
    112.                     
    113.                     // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
    114.                     const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
    115.                     
    116.                     // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
    117.                     const int dist = DescriptorDistance(d1,d2);
    118.                     
    119.                     if(dist>TH_LOW || dist>bestDist)
    120.                         continue;
    121.  
    122.                     // 通过特征点索引idx2在pKF2中取出对应的特征点
    123.                     const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
    124.  
    125.                     //? 为什么双目就不需要判断像素点到极点的距离的判断？
    126.                     // 因为双目模式下可以左右互匹配恢复三维点
    127.                     if(!bStereo1 && !bStereo2)
    128.                     {
    129.                         const float distex = ex-kp2.pt.x;
    130.                         const float distey = ey-kp2.pt.y;
    131.                         // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
    132.                         // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
    133.                         // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
    134.                         //? 对这个阈值的有效性持怀疑态度
    135.                         if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
    136.                             continue;
    137.                     }
    138.  
    139.                     // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
    140.                     if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
    141.                     {
    142.                         // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
    143.                         bestIdx2 = idx2;
    144.                         bestDist = dist;
    145.                     }
    146.                 }
    147.  
    148.                 
    149.                 if(bestIdx2>=0)
    150.                 {
    151.                     const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
    152.                     // 记录匹配结果
    153.                     vMatches12[idx1]=bestIdx2;      
    154.                     
    155.                     vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
    156.                     nmatches++;
    157.  
    158.                     // 记录旋转差直方图信息
    159.                     if(mbCheckOrientation)
    160.                     {
    161.                         // angle：角度，表示匹配点对的方向差。
    162.                         float rot = kp1.angle-kp2.angle;
    163.                         if(rot<0.0)
    164.                             rot+=360.0f;
    165.                         int bin = round(rot*factor);
    166.                         if(bin==HISTO_LENGTH)
    167.                             bin=0;
    168.                         assert(bin>=0 && bin
    169.                         rotHist[bin].push_back(idx1);
    170.                     }
    171.                 }
    172.             }
    173.  
    174.             f1it++;
    175.             f2it++;
    176.         }
    177.         else if(f1it->first < f2it->first)
    178.         {
    179.             f1it = vFeatVec1.lower_bound(f2it->first);
    180.         }
    181.         else
    182.         {
    183.             f2it = vFeatVec2.lower_bound(f1it->first);
    184.         }
    185.     }
    186.  
    187.     // Step 3 用旋转差直方图来筛掉错误匹配对
    188.     if(mbCheckOrientation)
    189.     {
    190.         int ind1=-1;
    191.         int ind2=-1;
    192.         int ind3=-1;
    193.  
    194.         ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
    195.  
    196.         for(int i=0; i
    197.         {
    198.             if(i==ind1 || i==ind2 || i==ind3)
    199.                 continue;
    200.             for(size_t j=0, jend=rotHist[i].size(); j
    201.             {              
    202.                 vbMatched2[vMatches12[rotHist[i][j]]] = false;  // !清除匹配关系。原作者漏掉！
    203.                 vMatches12[rotHist[i][j]]=-1;
    204.                 nmatches--;
    205.             }
    206.         }
    207.  
    208.     }
    209.  
    210.     // Step 4 存储匹配关系，下标是关键帧1的特征点id，存储的是关键帧2的特征点id
    211.     vMatchedPairs.clear();
    212.     vMatchedPairs.reserve(nmatches);
    213.  
    214.     for(size_t i=0, iend=vMatches12.size(); i
    215.     {
    216.         if(vMatches12[i]<0)
    217.             continue;
    218.         vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
    219.     }
    220.  
    221.     return nmatches;
    222. }

    2.2 关于基础矩阵 

            在第一帧的坐标系下，设P的空间位置为 P=[X,Y,Z]^{T}        

            根据针孔相机模型，两个像素点p_{1},p_{2}的像素位置为：s_{1}p_{1}=KP \ ,s_{2}p_{2}=K(R_{21}P+t_{21})，这里K为相机内参矩阵，R,t为两个坐标系的相机运动。

            (注意：为什么是R_{21}，因为P是世界坐标即在O_{1}参考系下看见的坐标，但不知道第二个坐标系看到的P是什么样子，于是乘以R_{21}，转化为第二个坐标系即O_{2}看到的坐标值，那s_{2}是什么，s_{2}是O_{2}p，p_{2}是在O_{2}参考系下的像素矩阵)

            具体来说，这里计算的是R_{21}和t_{21}，因为它们把第一个坐标系下的坐标转换到第二个坐标系下。也可以把它们写成李代数形式。
            有时，我们会使用齐次坐标表示像素点。在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如，s_{1}p_{1}和p_{1}成投影关系，它们在齐次坐标的意义下是相等的。我们称这种相等关系为尺度意义下相等(equal up to a scale)，记作：sp\simeq p

            那么，上述两个投影关系可写为：  p_{1}\simeq KP \ ,p_{2}\simeq K(RP+t)

            现在取 x_{1}=K^{-1}p_{1},x_{2}=K^{-1}p_{2}

            这里的x_{1},x_{2}是两个像素点的归一化平面上的坐标。代入上式，得x_{2}\simeq Rx_{1}+t

            两边同时左乘\hat{t}。回忆^的定义，这相当于两侧同时与t做外积：t^{\wedge }x_{2}\simeq t^{\wedge }Rx_{1}

            两侧同时左乘x_{2}^{T}：x_{2}^{T}t^{\wedge }x_{2}\simeq x_{2}^{T}t^{\wedge }Rx_{1}

            等式左侧，t^{\wedge }x_{2}是一个与t^{\wedge }和x_{2}都垂直的向量。它再和x_{2}做内积时，将得到0。

            由于等式左侧严格为零，乘以任意非零常数之后也为零，于是我们可以把\simeq写成通常的等号。因此，我们就得到：x_{2}^{T}t^{\wedge }Rx_{1} = 0

            重新带入p_{1},p_{2}有p_{2}^{T}K^{-T}t^{\wedge }RK^{-1}p_{1} = 0

            这两个式子都称为对极约束。它的几何意义是O_{1},P,O_{2}三者共面。

            对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵：基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E，于是可以进一步简化对极约束：

                                    

    E=t^{\wedge }R,\ F=K^{-T}EK^{-1},\ x_{2}Ex_{1}=p_{2}^{T}Fp_{1}=0

    3.总结

            对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：

    1.根据配对点的像素位置求出E或者F。
    2.根据E或者F求出R,t。
            由于E和F只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中往往使用形式更简单的E。我们以E为例，介绍上面两个问题如何求解。

    4.本质矩阵的特性

            根据定义，本质矩阵E=t^{\wedge }R。它是一个3×3的矩阵，内有9个未知数。那么，是不是任意一个3×3的矩阵都可以被当成本质矩阵呢？
            本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束依然满足。我们把这件事情称为E在不同尺度下是等价的。
            根据E=t^{\wedge }R，可以证明，本质矩阵E的奇异值必定是[\sigma ,\sigma ,0]^{T}的形式。这称为本质矩阵的内在性质。
            另外，由于平移和旋转各有3个自由度，故t^{\wedge }R共有6个自由度。但由于尺度等价性，故E实际上有5个自由度。

    5.八点法求本质矩阵

            E具有5个自由度的事实，表明我们最少可以用5对点来求解E。但是，E的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计E——这就是经典的八点法（Eight-point-algorithm ) 。八点法只利用了E的线性性质，因此可以在线性代数框架下求解。下面我们来看八点法是如何工作的：
            考虑一对匹配点，它们的归一化坐标为x_{1}=[u_{1},v_{1},1]^{T},x_{2}=[u_{2},v_{2},1]^{T}。根据对极约束，有

                                            (u_{2},v_{2},1)\begin{pmatrix} e_{1} & e_{2}&e_{3} \\ e_{4} &e_{5} & e_{6}\\ e_{7} & e_{8}& e_{9} \end{pmatrix} \begin{pmatrix} u_{1}\\v_{1} \\ 1 \end{pmatrix}=0

            我们把矩阵E展开，写成向量的形式：e=(e_{1},e_{2},e_{3},e_{4},e_{5},e_{6},e_{7},e_{8},e_{9})^{T}

            那么，对极约束可以写成与e有关的线性形式：                        ​​​​​        

    [u_{2}u_{1},u_{2}v_{1},u_{2},v_{2}u_{1},v_{2}v_{1},v_{2},u_{1},v_{1},1]\cdot e=0

            同理，对于其他点对也有相同的表示。我们把所有点都放到一个方程中，变成线性方程组(u^{i},v^{i}表示第i个特征点,依此类推)：

            这8个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为8×9。

            e位于该矩阵的零空间中。如果系数矩阵是满秩的(即秩为8)，那么它的零空间维数为1，也就是e构成一条线。这与e的尺度等价性是一致的。如果8对匹配点组成的矩阵满足秩为8的条件，那么E的各元素就可由上述方程解得。

    6.根据本质矩阵估计相机运用参数R,t

             接下来的问题是如何根据已经估得的本质矩阵E,恢复出相机的运动R,t。这个过程是由奇异值分解（SVD）得到的。设E的SVD为：E=U\Sigma V^{T}

             其中U,V为正交阵，\Sigma为奇异值矩阵。根据E的内在性质，我们知道\Sigma =diag(\sigma ,\sigma ,0)。在SVD分解中，对于任意一个E，存在两个可能的t,R与它对应：

            

             其中，R_{Z}(\frac{\pi }{2})表示沿Z轴旋转90°得到旋转矩阵。同时，由于-E和E等价，所以对任意一个t取负号，也会得到同样的结果。因此，从E分解到t,R时，一共存在4个可能的解。
            下图展示了分解本质矩阵得到的4个解。我们已知空间点在相机(蓝色线)上的投影(红色点)，想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况:


            只有第一种解中P在两个相机中都具有正的深度。因此，只要把任意一点代入4种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

             如果利用E的内在性质，那么它只有5个自由度。所以最少可以通过5对点来求解相机运动。然而这种做法形式复杂，从工程实现角度考虑，由于平时通常会有几十对乃至上百对的匹配点，从8对减至5对意义并不明显。为保持简单，我们这里就只介绍基本的八点法。
            剩下的一个问题是：根据线性方程解出的E，可能不满足E的内在性质——它的奇异值不一定为\sigma ,\sigma ,0的形式。这时，我们会刻意地把矩阵调整成上面的样子。通常的做法是，对八点法求得的E进行SVD，会得到奇异值矩阵\Sigma =diag(\sigma_{1} ,\sigma_{2} ,\sigma _{3})，不妨设\sigma_{1}\geqslant \sigma_{2}\geqslant \sigma _{3}。取:

                    ​​​​​​​

             这相当于是把求出来的矩阵投影到了E所在的流形上。当然，更简单的做法是将奇异值矩阵取成diag(1,1,0)，因为E具有尺度等价性，所以这样做也是合理的。

    2.3  计算KF1的相机中心在KF2图像平面的二维像素坐标

    1.     const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    2.     const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
    3.  
    4.     // Compute epipole in second image
    5.     // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
    6.     // KF1相机光心在世界坐标系坐标Cw
    7.     cv::Mat Cw = pKF1->GetCameraCenter(); 
    8.     // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
    9.     cv::Mat R2w = pKF2->GetRotation();    
    10.     cv::Mat t2w = pKF2->GetTranslation(); 
    11.     // KF1的相机光心转化到KF2坐标系中的坐标
    12.     cv::Mat C2 = R2w*Cw+t2w; 
    13.     const float invz = 1.0f/C2.at<float>(2);
    14.     // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
    15.     const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
    16.     const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;
    17.  
    18.     // Find matches between not tracked keypoints
    19.     // Matching speed-up by ORB Vocabulary
    20.     // Compare only ORB that share the same node
    21.  
    22.     int nmatches=0;
    23.     // 记录匹配是否成功，避免重复匹配
    24.     vector<bool> vbMatched2(pKF2->N,false);        
    25.     vector<int> vMatches12(pKF1->N,-1);
    26.     // 用于统计匹配点对旋转差的直方图
    27.     vector<int> rotHist[HISTO_LENGTH];
    28.     for(int i=0;i
    29.         rotHist[i].reserve(500);
    30.  
    31.      //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
    32.     const float factor = HISTO_LENGTH/360.0f;

            建立指向帧1和帧2的FeatureVector迭代器vFeatVec1 、vFeatVec2。

            我们计算KF1的相机中心在KF2图像平面的二维像素坐标即e2。步骤如下：

            1.获取KF1相机光心在世界坐标系坐标Cw。

            2.获取KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系。

            3.计算KF1的相机光心转化到KF2坐标系中的坐标C2 。

            4.计算极点坐标ex，ey。

    2.4 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配 

    1. // Step 2 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配
    2.     // FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历
    3.     // FeatureVector的数据结构类似于：{(node1,feature_vector1) (node2,feature_vector2)...}
    4.     // f1it->first对应node编号，f1it->second对应属于该node的所有特特征点编号
    5.     DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
    6.     DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
    7.     DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
    8.     DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
    9.  
    10.     // Step 2.1：遍历pKF1和pKF2中的node节点
    11.     while(f1it!=f1end && f2it!=f2end)
    12.     {
    13.         // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
    14.         if(f1it->first == f2it->first)
    15.         {
    16.             // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
    17.             for(size_t i1=0, iend1=f1it->second.size(); i1
    18.             {
    19.                 // 获取帧1即pKF1中属于该node节点的描述子索引 0 --- 数量-1
    20.                 const size_t idx1 = f1it->second[i1];
    21.                 
    22.                 // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
    23.                 MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
    24.                 
    25.                 // If there is already a MapPoint skip
    26.                 // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
    27.                 if(pMP1)
    28.                     continue;
    29.  
    30.                 // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
    31.                 const bool bStereo1 = pKF1->mvuRight[idx1]>=0;
    32.  
    33.                 if(bOnlyStereo)
    34.                     if(!bStereo1)
    35.                         continue;
    36.                 
    37.                 // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
    38.                 const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
    39.                 
    40.                 // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
    41.                 const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
    42.                 
    43.                 int bestDist = TH_LOW;
    44.                 int bestIdx2 = -1;
    45.                 
    46.                 // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
    47.                 for(size_t i2=0, iend2=f2it->second.size(); i2
    48.                 {
    49.                     // 获取pKF2中属于该node节点的所有特征点索引
    50.                     size_t idx2 = f2it->second[i2];
    51.                     
    52.                     // 通过特征点索引idx2在pKF2中取出对应的MapPoint
    53.                     MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
    54.                     
    55.                     // If we have already matched or there is a MapPoint skip
    56.                     // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
    57.                     if(vbMatched2[idx2] || pMP2)
    58.                         continue;
    59.  
    60.                     const bool bStereo2 = pKF2->mvuRight[idx2]>=0;
    61.  
    62.                     if(bOnlyStereo)
    63.                         if(!bStereo2)
    64.                             continue;
    65.                     
    66.                     // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
    67.                     const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
    68.                     
    69.                     // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
    70.                     const int dist = DescriptorDistance(d1,d2);
    71.                     
    72.                     if(dist>TH_LOW || dist>bestDist)
    73.                         continue;
    74.  
    75.                     // 通过特征点索引idx2在pKF2中取出对应的特征点
    76.                     const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
    77.  
    78.                     //? 为什么双目就不需要判断像素点到极点的距离的判断？
    79.                     // 因为双目模式下可以左右互匹配恢复三维点
    80.                     if(!bStereo1 && !bStereo2)
    81.                     {
    82.                         const float distex = ex-kp2.pt.x;
    83.                         const float distey = ey-kp2.pt.y;
    84.                         // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
    85.                         // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
    86.                         // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
    87.                         //? 对这个阈值的有效性持怀疑态度
    88.                         if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
    89.                             continue;
    90.                     }
    91.  
    92.                     // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
    93.                     if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
    94.                     {
    95.                         // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
    96.                         bestIdx2 = idx2;
    97.                         bestDist = dist;
    98.                     }
    99.                 }
    100.  
    101.                 
    102.                 if(bestIdx2>=0)
    103.                 {
    104.                     const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
    105.                     // 记录匹配结果
    106.                     vMatches12[idx1]=bestIdx2;      
    107.                     
    108.                     vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
    109.                     nmatches++;
    110.  
    111.                     // 记录旋转差直方图信息
    112.                     if(mbCheckOrientation)
    113.                     {
    114.                         // angle：角度，表示匹配点对的方向差。
    115.                         float rot = kp1.angle-kp2.angle;
    116.                         if(rot<0.0)
    117.                             rot+=360.0f;
    118.                         int bin = round(rot*factor);
    119.                         if(bin==HISTO_LENGTH)
    120.                             bin=0;
    121.                         assert(bin>=0 && bin
    122.                         rotHist[bin].push_back(idx1);
    123.                     }
    124.                 }
    125.             }
    126.  
    127.             f1it++;
    128.             f2it++;
    129.         }
    130.         else if(f1it->first < f2it->first)
    131.         {
    132.             f1it = vFeatVec1.lower_bound(f2it->first);
    133.         }
    134.         else
    135.         {
    136.             f2it = vFeatVec2.lower_bound(f1it->first);
    137.         }
    138.     }

             我们建立两帧图像的FeatureVector迭代器，因为只有同一个node节点特征点才可能进行匹配，于是我们先判断帧一的初始节点起始位置和帧二的起始节点是否相同，若不同，执行else if或者else循环先使迭代器指向同一节点位置。

            然后依次遍历该node_id下的每个特征点，先取出该特征点的索引值idx1，然后根据索引值一次取得地图点pMP1和特征点kp1，然后取出该特征点的描述子d1，对第二帧也是如此进行，然后计算两特征点的描述子满足阈值要求取出特征点进行一系列的判断。然后就和之前的一样了

  • 相关阅读:
    交通物流模型 | Python实现基于张量分解的交通流量时空模式挖掘（出租车车载GPS数据、公交卡刷卡数据、POI的分布数据）
    【校招VIP】产品基础知识之学习渠道
    Java 24 Design Pattern 之 工厂模式
    聚美优品API 根据关键词取商品列表 Onebound电商平台数据
    终端常用脚本命令
    Linux之网络协议概念
    【科研新手指南4】ChatGPT的prompt技巧 心得
    Android 11.0 默认授予app获取序列号SerialNo权限
    ShiroFilter
    linux搭建java部署环境-docker
  • 原文地址：https://blog.csdn.net/qq_41694024/article/details/126332164