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

    目录

    1.函数作用

    2.函数解析 

    2.0 函数代码

    2.1 参数解析 

    2.2  构建旋转直方图解析

    2.3 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 

    2.4 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的

    2.5 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配 

    2.6 计算匹配点旋转角度差所在的直方图

     2.7 筛除旋转直方图中“非主流”部分

    2.8 将最后通过筛选的匹配好的特征点保存到vbPrevMatched 

    1.函数作用

            用于单目初始化中用于参考帧和当前帧的特征点匹配。

    2.函数解析 

    2.0 函数代码

    1. /**
    2.  * @brief 单目初始化中用于参考帧和当前帧的特征点匹配
    3.  * 步骤
    4.  * Step 1 构建旋转直方图
    5.  * Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
    6.  * Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
    7.  * Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
    8.  * Step 5 计算匹配点旋转角度差所在的直方图
    9.  * Step 6 筛除旋转直方图中“非主流”部分
    10.  * Step 7 将最后通过筛选的匹配好的特征点保存
    11.  * @param[in] F1                        初始化参考帧                  
    12.  * @param[in] F2                        当前帧
    13.  * @param[in & out] vbPrevMatched       本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标
    14.  * @param[in & out] vnMatches12         保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
    15.  * @param[in] windowSize                搜索窗口
    16.  * @return int                          返回成功匹配的特征点数目
    17.  */
    18. int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
    19. {
    20.     int nmatches=0;
    21.     // F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间
    22.     vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);
    23.  
    24.     // Step 1 构建旋转直方图，HISTO_LENGTH = 30
    25.     vector<int> rotHist[HISTO_LENGTH];
    26.     for(int i=0;i
    27.     // 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量
    28.         rotHist[i].reserve(500);   
    29.  
    30.     //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
    31.     const float factor = HISTO_LENGTH/360.0f;
    32.  
    33.     // 匹配点对距离，注意是按照F2特征点数目分配空间
    34.     vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
    35.     // 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间
    36.     vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
    37.  
    38.     // 遍历帧1中的所有特征点
    39.     for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1
    40.     {
    41.         cv::KeyPoint kp1 = F1.mvKeysUn[i1];
    42.         int level1 = kp1.octave;
    43.         // 只使用原始图像上提取的特征点
    44.         if(level1>0)
    45.             continue;
    46.  
    47.         // Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
    48.         // vbPrevMatched 输入的是参考帧 F1的特征点
    49.         // windowSize = 100，输入最大最小金字塔层级 均为0
    50.         vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
    51.  
    52.         // 没有候选特征点，跳过
    53.         if(vIndices2.empty())
    54.             continue;
    55.  
    56.         // 取出参考帧F1中当前遍历特征点对应的描述子
    57.         cv::Mat d1 = F1.mDescriptors.row(i1);
    58.  
    59.         int bestDist = INT_MAX;     //最佳描述子匹配距离，越小越好
    60.         int bestDist2 = INT_MAX;    //次佳描述子匹配距离
    61.         int bestIdx2 = -1;          //最佳候选特征点在F2中的index
    62.  
    63.         // Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
    64.         for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
    65.         {
    66.             size_t i2 = *vit;
    67.             // 取出候选特征点对应的描述子
    68.             cv::Mat d2 = F2.mDescriptors.row(i2);
    69.             // 计算两个特征点描述子距离
    70.             int dist = DescriptorDistance(d1,d2);
    71.  
    72.             if(vMatchedDistance[i2]<=dist)
    73.                 continue;
    74.             // 如果当前匹配距离更小，更新最佳次佳距离
    75.             if(dist
    76.             {
    77.                 bestDist2=bestDist;
    78.                 bestDist=dist;
    79.                 bestIdx2=i2;
    80.             }
    81.             else if(dist
    82.             {
    83.                 bestDist2=dist;
    84.             }
    85.         }
    86.  
    87.         // Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
    88.         // 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值
    89.         if(bestDist<=TH_LOW)
    90.         {
    91.             // 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高
    92.             if(bestDist<(float)bestDist2*mfNNratio)
    93.             {
    94.                 // 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉
    95.                 if(vnMatches21[bestIdx2]>=0)
    96.                 {
    97.                     vnMatches12[vnMatches21[bestIdx2]]=-1;
    98.                     nmatches--;
    99.                 }
    100.                 // 次优的匹配关系，双向建立
    101.                 // vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
    102.                 vnMatches12[i1]=bestIdx2;
    103.                 vnMatches21[bestIdx2]=i1;
    104.                 vMatchedDistance[bestIdx2]=bestDist;
    105.                 nmatches++;
    106.  
    107.                 // Step 5 计算匹配点旋转角度差所在的直方图
    108.                 if(mbCheckOrientation)
    109.                 {
    110.                     // 计算匹配特征点的角度差，这里单位是角度°，不是弧度
    111.                     float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
    112.                     if(rot<0.0)
    113.                         rot+=360.0f;
    114.                     // 前面factor = HISTO_LENGTH/360.0f 
    115.                     // bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin  
    116.                     int bin = round(rot*factor);
    117.                     // 如果bin 满了又是一个轮回
    118.                     if(bin==HISTO_LENGTH)
    119.                         bin=0;
    120.                     assert(bin>=0 && bin
    121.                     rotHist[bin].push_back(i1);
    122.                 }
    123.             }
    124.         }
    125.  
    126.     }
    127.  
    128.     // Step 6 筛除旋转直方图中“非主流”部分
    129.     if(mbCheckOrientation)
    130.     {
    131.         int ind1=-1;
    132.         int ind2=-1;
    133.         int ind3=-1;
    134.         // 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
    135.         ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
    136.  
    137.         for(int i=0; i
    138.         {
    139.             if(i==ind1 || i==ind2 || i==ind3)
    140.                 continue;
    141.             // 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向”    
    142.             for(size_t j=0, jend=rotHist[i].size(); j
    143.             {
    144.                 int idx1 = rotHist[i][j];
    145.                 if(vnMatches12[idx1]>=0)
    146.                 {
    147.                     vnMatches12[idx1]=-1;
    148.                     nmatches--;
    149.                 }
    150.             }
    151.         }
    152.  
    153.     }
    154.  
    155.     //Update prev matched
    156.     // Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatched
    157.     for(size_t i1=0, iend1=vnMatches12.size(); i1
    158.         if(vnMatches12[i1]>=0)
    159.             vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
    160.  
    161.     return nmatches;
    162. }

    2.1 参数解析 

    1. /**
    2.  * @brief 单目初始化中用于参考帧和当前帧的特征点匹配
    3.  * 步骤
    4.  * Step 1 构建旋转直方图
    5.  * Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
    6.  * Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
    7.  * Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
    8.  * Step 5 计算匹配点旋转角度差所在的直方图
    9.  * Step 6 筛除旋转直方图中“非主流”部分
    10.  * Step 7 将最后通过筛选的匹配好的特征点保存
    11.  * @param[in] F1                        初始化参考帧                  
    12.  * @param[in] F2                        当前帧
    13.  * @param[in & out] vbPrevMatched       本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标
    14.  * @param[in & out] vnMatches12         保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
    15.  * @param[in] windowSize                搜索窗口
    16.  * @return int                          返回成功匹配的特征点数目
    17.  */
    18. int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector &vbPrevMatched, vector<int> &vnMatches12, int windowSize)

            本函数的输入为两帧图片Frame &F1，Frame &F2及搜索窗口大小windowSize，输出为特征点的匹配关系vnMatches12、vbPrevMatched。

    vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);

            我们可以看到，vnMatches12是记录匹配关系的，大小定为帧1中经过去畸变后得到的特征点的数量。具体含义举个例子，比如vnMatches12[0] = 4的含义就是帧1中的第一个特征点和帧2中1的第5个特征点成功匹配。 刚开始我们全都初始化为-1说明匹配关系为空。

            vbPrevMatched作为输出，记录的是匹配成功的点坐标信息。即如果第一帧的第五个特征点成功匹配到第二帧的第五十个特征点。vbPrevMatched[4]中储存的值为第二帧第五十个特征点的坐标。

            用nmatches变量记录了已经匹配的特征点数目。

    2.2  构建旋转直方图解析

    1.  // Step 1 构建旋转直方图，HISTO_LENGTH = 30
    2.     vector<int> rotHist[HISTO_LENGTH];
    3.     for(int i=0;i
    4.     // 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量
    5.         rotHist[i].reserve(500);   
    6.  
    7.     //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
    8.     const float factor = HISTO_LENGTH/360.0f;
    9.  
    10.     // 匹配点对距离，注意是按照F2特征点数目分配空间
    11.     vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
    12.     // 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间
    13.     vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
    14.  
    15.     // 遍历帧1中的所有特征点
    16.     for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1
    17.     {
    18.         cv::KeyPoint kp1 = F1.mvKeysUn[i1];
    19.         int level1 = kp1.octave;
    20.         // 只使用原始图像上提取的特征点
    21.         if(level1>0)
    22.             continue;

            这段代码是在做什么呢？

            挑选出在旋转角度差落在在直方图区间内数量最多的前三个直方图区间bin的索引，然后剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向” 。

              我们目标是建立如上的直方图，直方图分为两部分：每一个区间及每个区间所含的样本数量。

            我们用vector rotHist[HISTO_LENGTH] 来构建了HISTO_LENGTH个区间，rotHist[i] 就是每个区间含的样本数量。（好奇为什么不用双层vector或者pair对组）。并初始化每个区间的样本数量初值为500。

            匹配点对的汉明距离（描述子距离）vMatchedDistance，这里是按帧2初始化的，又定义了反向匹配vnMatches21，然后我们遍历第一帧的所有第一层特征点。

    2.3 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 

    1. // Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
    2.         // vbPrevMatched 输入的是参考帧 F1的特征点
    3.         // windowSize = 100，输入最大最小金字塔层级 均为0
    4.         vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
    5.  
    6.         // 没有候选特征点，跳过
    7.         if(vIndices2.empty())
    8.             continue;
    9.  
    10.         // 取出参考帧F1中当前遍历特征点对应的描述子
    11.         cv::Mat d1 = F1.mDescriptors.row(i1);
    12.  
    13.         int bestDist = INT_MAX;     //最佳描述子匹配距离，越小越好
    14.         int bestDist2 = INT_MAX;    //次佳描述子匹配距离
    15.         int bestIdx2 = -1;          //最佳候选特征点在F2中的index

     参考链接：如何把去畸变的特征点分到栅格中

    ORB-SLAM2 ---- Frame::AssignFeaturesToGrid函数_Courage2022的博客-CSDN博客https://blog.csdn.net/qq_41694024/article/details/126316590

    参考链接： GetFeaturesInArea函数解析

    ORB-SLAM2 ---- Frame::GetFeaturesInArea 函数_Courage2022的博客-CSDN博客https://blog.csdn.net/qq_41694024/article/details/126319616?csdn_share_tail=%7B%22type%22%3A%22blog%22%2C%22rType%22%3A%22article%22%2C%22rId%22%3A%22126319616%22%2C%22source%22%3A%22qq_41694024%22%7D

             我们用vIndices2 变量接收所有帧一的正在迭代某个特征点的候选特征点kp1 匹配索引的集合，若vIndices2 为空，则帧2中没有与kp1匹配的候选特征点，这时需要对帧1的下一个特征点匹配候选特征点进行匹配。若帧2中有与kp1匹配的候选特征点，则取出参考帧F1中当前遍历特征点对应的描述子d1。

    2.4 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的

    1.         for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
    2.         {
    3.             size_t i2 = *vit;
    4.             // 取出候选特征点对应的描述子
    5.             cv::Mat d2 = F2.mDescriptors.row(i2);
    6.             // 计算两个特征点描述子距离
    7.             int dist = DescriptorDistance(d1,d2);
    8.  
    9.             if(vMatchedDistance[i2]<=dist)
    10.                 continue;
    11.             // 如果当前匹配距离更小，更新最佳次佳距离
    12.             if(dist
    13.             {
    14.                 bestDist2=bestDist;
    15.                 bestDist=dist;
    16.                 bestIdx2=i2;
    17.             }
    18.             else if(dist
    19.             {
    20.                 bestDist2=dist;
    21.             }
    22.         }

            取出每一个帧2中候选的特征点的描述子，计算帧一中特征点kp1与帧二中每个候选特征点的汉明距离，保存最佳和次加候选特征点的汉明距离和特征点索引。

    2.5 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配 

    2.6 计算匹配点旋转角度差所在的直方图

    1.         if(bestDist<=TH_LOW)
    2.         {
    3.             // 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高
    4.             if(bestDist<(float)bestDist2*mfNNratio)
    5.             {
    6.                 // 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉
    7.                 if(vnMatches21[bestIdx2]>=0)
    8.                 {
    9.                     vnMatches12[vnMatches21[bestIdx2]]=-1;
    10.                     nmatches--;
    11.                 }
    12.                 // 次优的匹配关系，双向建立
    13.                 // vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
    14.                 vnMatches12[i1]=bestIdx2;
    15.                 vnMatches21[bestIdx2]=i1;
    16.                 vMatchedDistance[bestIdx2]=bestDist;
    17.                 nmatches++;
    18.  
    19.                 // Step 5 计算匹配点旋转角度差所在的直方图
    20.                 if(mbCheckOrientation)
    21.                 {
    22.                     // 计算匹配特征点的角度差，这里单位是角度°，不是弧度
    23.                     float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
    24.                     if(rot<0.0)
    25.                         rot+=360.0f;
    26.                     // 前面factor = HISTO_LENGTH/360.0f 
    27.                     // bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin  
    28.                     int bin = round(rot*factor);
    29.                     // 如果bin 满了又是一个轮回
    30.                     if(bin==HISTO_LENGTH)
    31.                         bin=0;
    32.                     assert(bin>=0 && bin
    33.                     rotHist[bin].push_back(i1);
    34.                 }
    35.             }
    36.         }
    1. // 要用到的一些阈值
    2. const int ORBmatcher::TH_HIGH = 100;
    3. const int ORBmatcher::TH_LOW = 50;
    4. const int ORBmatcher::HISTO_LENGTH = 30;

            并不是上一步中得到的由汉明距离匹配的特征点就是最终匹配的特征点，还要满足以下条件：

            如果最佳匹配点的汉明距离大于TH_LOW，则匹配失败。（汉明距离大意味着匹配精度低）

            如果最佳匹配点的汉明距离/次佳匹配点的汉明距离大于一定的比例，则匹配失败（保证特征点的辨识度）

            如果候选的特征点已经和帧一之前的某个特征点匹配过了，则匹配失败（两次一定有一次是误匹配）

            如果满足这三点，则”暂时“认为特征点匹配成功，更新匹配容器vnMatches12、vnMatches21、vMatchedDistance表明匹配成功，并将成功匹配特征点的数量nmatches+1。

     bool mbCheckOrientation;    ///< 是否检查特征点的方向

            若此标志位为true，我们还要检查特征点的方向构建旋转方向直方图再剔除一部分匹配好的特征点作为最终匹配的特征点输出给上层调用。

            关于特征点的angle属性如何计算及意义是什么，我在之前的博客有说，放到了下面链接中。

            我们计算两个特征点的angle差rot，使其在0°-360°之间。

            我们前面定义了factor = HISTO_LENGTH/360.0f，这里HISTO_LENGTH是分布直方图的区间数目，这里factor * rot = 角度/360° * 分布直方图的区间数目，即匹配的特征点的角度差落在哪个分布直方图的区间内，然后将帧2中匹配的特征点放入直方图的一个区间内。

            举一个例子吧：

            如果计算角度差值为5° 则5/360 * 30 = 0.42 放入第0个直方图内。

            如果计算角度差值为15° 则15/360 * 30 = 1.26 放入第1个直方图内。

            如果计算角度差值为140° 则140/360 * 30 = 11.67 放入第11个直方图内。​​​​​​​ORB-SLAM2 ---- IC_Angle函数_Courage2022的博客-CSDN博客https://blog.csdn.net/qq_41694024/article/details/126306830

     2.7 筛除旋转直方图中“非主流”部分

    1.     if(mbCheckOrientation)
    2.     {
    3.         int ind1=-1;
    4.         int ind2=-1;
    5.         int ind3=-1;
    6.         // 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
    7.         ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
    8.  
    9.         for(int i=0; i
    10.         {
    11.             if(i==ind1 || i==ind2 || i==ind3)
    12.                 continue;
    13.             // 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向”    
    14.             for(size_t j=0, jend=rotHist[i].size(); j
    15.             {
    16.                 int idx1 = rotHist[i][j];
    17.                 if(vnMatches12[idx1]>=0)
    18.                 {
    19.                     vnMatches12[idx1]=-1;
    20.                     nmatches--;
    21.                 }
    22.             }
    23.         }
    24.  
    25.     }
     bool mbCheckOrientation;    ///< 是否检查特征点的方向

            若此标志位为true，我们还要检查特征点的方向构建旋转方向直方图再剔除一部分匹配好的特征点作为最终匹配的特征点输出给上层调用。

            这块就是挑选出在旋转角度差落在在直方图区间内数量最多的前三个直方图区间bin的索引，然后剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向” 。

             这是因为两张图片各个特征点在计算angle属性时各个特征点都是要计算这个旋转到x轴的角度的，那么它们的差值肯定趋于一致，若差别较大，则考虑特征点误匹配的几率大大提高。

    ORB-SLAM2 ---- ORBmatcher::ComputeThreeMaxima函数_Courage2022的博客-CSDN博客https://blog.csdn.net/qq_41694024/article/details/126321635?csdn_share_tail=%7B%22type%22%3A%22blog%22%2C%22rType%22%3A%22article%22%2C%22rId%22%3A%22126321635%22%2C%22source%22%3A%22qq_41694024%22%7D

    2.8 将最后通过筛选的匹配好的特征点保存到vbPrevMatched 

    1.     for(size_t i1=0, iend1=vnMatches12.size(); i1
    2.         if(vnMatches12[i1]>=0)
    3.             vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
    4.  
    5.     return nmatches;

            若帧1到帧2中帧1的某个特征点成功匹配到帧2的某个特征点，则更新vbPrevMatched容器的成功匹配帧一到帧二特征点的点坐标信息。

            即如果第一帧的第五个特征点成功匹配到第二帧的第五十个特征点。

            vbPrevMatched[4]中储存的值为第二帧第五十个特征点的坐标。

            最后返回匹配成功的特征点对数给上层函数

  • 相关阅读:
    AHB、APB详解和FCLK,PCLK,HCLK详解
    Python高级二
    【Python爬虫】8大模块md文档集合从0到scrapy高手，第7篇：selenium 数据提取详解
    Sality 病毒的驱动模块分析报告
    设计模式-适配器模式在Java中应用
    前端常用的编辑器、框架以及一些常用工具，帮助前端开发者更快成长。
    Python 接口自动化测试详解
    Rdt2.1 和 Rdt2.2的详细解释
    代码整洁之原则
    AIGC ChatGPT 4 将数据接口文件使用Python进行入库Mysql
  • 原文地址：https://blog.csdn.net/qq_41694024/article/details/126319167