ORB-SLAM2源码逐行解析系列（十五）：ORB-SLAM2特征匹配之基于Sim3变换的特征匹配

1. 通过Sim(3)变换进行相互投影匹配

（1）相互投影匹配的目的

​ 在闭环线程中，由于闭环候选帧与当前关键帧时间间隔较远，没有先验信息，因此最早是通过词袋进行搜索匹配的（SearchByBoW），但是词袋搜索会存在漏匹配。此时，可采用初步估计的Sim(3)位姿对当前关键帧和闭环候选关键帧进行相互投影匹配，目的是生成更多的匹配点对。

（2）相互投影匹配的思路

• 假设待匹配的关键帧分别是KF1和KF2，先统计它们之间已经匹配好的特征点对，目的是在后续投影中跳过这些已经匹配好的特征点对，从剩下的未匹配的特征点中寻找新的匹配关系。
• 把KF1的地图点用Sim21投影到KF2上，在投影点附近一定范围内寻找候选匹配点，从中选择描述子距离最近的点作为最佳匹配点。
• 把KF2的地图点用Sim12投影到KF1上，在投影点附近一定范围内寻找候选匹配点，从中选择描述子距离最近的点作为最佳匹配点.
• 找出同时满足上述两个最佳匹配的特征点匹配对，作为最终可靠的新的匹配结果。

（3）相互投影匹配的大致流程

• 准备工作：获取相机内参，两关键帧的位姿和对应地图点，以及通过Sim(3)变换得到的两关键帧间的位姿变换
• 遍历两关键帧中的特征点，记录已经存在的匹配关系
• 遍历pKF1中的特征点，通过Sim(3)变换，寻找pKF1中特征点在pKF2中的新的匹配，具体如下：
  • 获取特征点对应的地图点，并判断地图点的有效性：存在、不为Bad，且不在已有地图点集合里
  • 将地图点通过Sim(3)变换投影到pKF2的像素坐标系下，并检查对应的像素点是否在图像范围内
  • 计算pKF2相机光心到地图点的距离，并以此预测出该地图点对应的特征点所在pKF2中的金字塔层数
  • 根据上面预测出的该地图点对应特征点所在pKF2中的金字塔层数，结合所设阈值，确定pKF2的搜索半径
  • 根据前面投影出的像素点以及计算出的搜索半径，获取pKF2上的候选匹配点
  • 遍历pKF2上的候选匹配点，在候选匹配点与地图点处于图像金字塔相邻层的情况下，获取最小的描述子距离及对应的候选匹配点索引
  • 若计算出的最小描述子距离小于设定的距离阈值，则将对应候选匹配点作为pKF1上当前点的最佳匹配点
• 遍历pKF2中的特征点，通过Sim(3)变换，寻找pKF2中特征点在pKF1中的新的匹配，具体如下：
  • 获取特征点对应的地图点，并判断地图点的有效性：存在、不为Bad，且不在已有地图点集合里
  • 将地图点通过Sim(3)变换投影到pKF1的像素坐标系下，并检查对应的像素点是否在图像范围内
  • 计算pKF1相机光心到地图点的距离，并以此预测出该地图点对应的特征点所在pKF1中的金字塔层数
  • 根据上面预测出的该地图点对应特征点所在pKF1中的金字塔层数，结合所设阈值，确定pKF1的搜索半径
  • 根据前面投影出的像素点以及计算出的搜索半径，获取pKF1上的候选匹配点
  • 遍历pKF1上的候选匹配点，在候选匹配点与地图点处于图像金字塔相邻层的情况下，获取最小的描述子距离及对应的候选匹配点索引
  • 若计算出的最小描述子距离小于设定的距离阈值，则将对应候选匹配点作为pKF12当前点的最佳匹配点
• 一致性检查，只有在两次相互匹配中都出现才能够认为是可靠的匹配

（4）相互投影匹配的代码实现

/**
 * @brief 通过Sim3变换，搜索两个关键帧中更多的匹配点对(之前使用SearchByBoW进行特征点匹配时会有漏匹配)
 * @param[in] pKF1              当前帧
 * @param[in] pKF2              闭环候选帧
 * @param[in] vpMatches12       i表示匹配的pKF1 特征点索引，vpMatches12[i]表示匹配的pKF2中地图点，null表示没有匹配
 * @param[in] s12               pKF2 到 pKF1 的Sim 变换中的尺度
 * @param[in] R12               pKF2 到 pKF1 的Sim 变换中的旋转矩阵
 * @param[in] t12               pKF2 到 pKF1 的Sim 变换中的平移向量
 * @param[in] th                搜索窗口的倍数
 * @return int                  新增的匹配点对数目
 */
int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, 
                             vector<MapPoint*> &vpMatches12,
                             const float &s12, const cv::Mat &R12, 
                             const cv::Mat &t12, const float th)
{
   
    // Step 1： 准备工作：内参，计算Sim3的逆
    const float &fx = pKF1->fx;
    const float &fy = pKF1->fy;
    const float &cx = pKF1->cx;
    const float &cy = pKF1->cy;

    // 获取pKF1的位姿：从world到camera1的变换
    cv::Mat R1w = pKF1->GetRotation();
    cv::Mat t1w = pKF1->GetTranslation();
    // 获取pKF2的位姿：从world到camera2的变换
    cv::Mat R2w = pKF2->GetRotation();
    cv::Mat t2w = pKF2->GetTranslation();
    
    // Sim3 的逆
    cv::Mat sR12 = s12*R12;
    cv::Mat sR21 = (1.0/s12)*R12.t();
    cv::Mat t21 = -sR21*t12;

    // 取出pKF1关键帧中的地图点
    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
    const int N1 = vpMapPoints1.size();
    // 取出pKF2关键帧中的地图点
    const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
    const int N2 = vpMapPoints2.size();

    // 记录pKF1，pKF2中已经匹配的特征点，已经匹配记为true，否则false
    vector