5、ORB-SLAM闭环检测之通过求解出来的sim3寻找当前关键帧和闭环候选帧之间的更多匹配

1、SearchBySim3

函数SearchBySim3（）。通过计算的Sim3变换，查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数，之前使用SearchByBoW进行特征点匹配时会有漏匹配，毕竟只是“替身”之间的匹配），投影搜索pKF1的特征点在pKF2中的匹配，同理，投影搜索pKF2的特征点在pKF1中的匹配，只有互相都成功匹配的才认为是可靠的匹配。
此函数所做的事，用图表示如下：

和往常一样，先把整个函数挂在下方，然后分别通过图讲解其实现原理。

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： 准备工作
    const float &fx = pKF1->fx;  
    const float &fy = pKF1->fy;
    const float &cx = pKF1->cx;
    const float &cy = pKF1->cy;

    // 从world到camera1的变换
    cv::Mat R1w = pKF1->GetRotation();        //取出当前关键帧的R t
    cv::Mat t1w = pKF1->GetTranslation();

    // 从world到camera2的变换
    cv::Mat R2w = pKF2->GetRotation();        //取出保留闭环候选关键帧的R t
    cv::Mat t2w = pKF2->GetTranslation();

    // Sim3 的逆
    cv::Mat sR12 = s12*R12;                   //sim3矩阵[sR t]   R12 闭环候选关键帧到当前关键帧的旋转矩阵        sim3的逆[（1/s）*R^T  （-1/s）*R^T*t]
    cv::Mat sR21 = (1.0/s12)*R12.t();         //        [0  1]   默认的sim3变换是候选到当前，求逆后，当前到候选          [     0           1        ]
    cv::Mat t21 = -sR21*t12;

    // 取出关键帧中的地图点
    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();//取出当前关键帧的地图点
    const int N1 = vpMapPoints1.size();//记录当前关键帧的地图点的尺寸

    const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();//取出保留闭环候选关键帧的地图点
    const int N2 = vpMapPoints2.size();//记录保留闭环候选关键帧的地图点的尺寸

    // pKF1中特征点的匹配情况，有匹配为true，否则false
    vector<bool> vbAlreadyMatched1(N1,false);//记录当前关键帧特征点匹配的结果（前面SearchByBoW，sim3匹配的结果）
    // pKF2中特征点的匹配情况，有匹配为true，否则false
    vector<bool> vbAlreadyMatched2(N2,false);//记录保留闭环候选关键帧特征点匹配的结果（前面SearchByBoW，sim3匹配的结果）

    // Step 2：记录已经匹配的特征点       //目的是为了把前面SearchByBoW匹配的结果，筛选出来，排除这些老的匹配点，去找新的匹配点
    for(int i=0; i<N1; i++) //循环当前关键帧特征点索引
    {
   
        MapPoint* pMP = vpMatches12[i];//取出当前循环的当前关键帧特征点索引对应匹配的保留闭环候选关键帧特征点的地图点
        if(pMP)//如果地图点存在，表明当前循环的当前关键帧特征点索引在对应匹配的保留闭环候选关键帧中已匹配
        {
   
            // pKF1中第i个特征点已经匹配成功
            vbAlreadyMatched1[i]=true;
            // 得到该地图点在关键帧pkF2 中的id
            int idx2 = pMP->GetIndexInKeyFrame(pKF2);//把这个地图点在对应匹配的保留闭环候选关键帧中的特征点索引取出来
            if(idx2>=0 && idx2<N2)//地图点在对应匹配的保留闭环候选关键帧中的特征点索引如果在0到N2范围内，表明保留闭环候选关键帧中的特征点索引在当前循环的当前关键帧中已匹配
                // pKF2中第idx2个特征点在pKF1中有匹配
                vbAlreadyMatched2[idx2]=true;   
        }
    }//这个for循环就是为了把前面SearchByBoW已经匹配的特征点记录一下

    vector<int> vnMatch1(N1,-1);//最终的当前关键帧与保留闭环候选关键帧匹配的特征点对
    vector<int> vnMatch2(N2,-1);//最终的保留闭环候选关键帧与当前关键帧匹配的特征点对

    // Step 3：通过Sim变换，寻找 pKF1 中特征点和 pKF2 中的新的匹配
    // 之前使用SearchByBoW进行特征点匹配时会有漏匹配
    for(int i1=0; i1<N1; i1++)//循环当前关键帧的特征点索引
    {
   
        MapPoint* pMP = vpMapPoints1[i1];//取出当前循环的当前关键帧的特征点索引对应的地图点

        // 该特征点已经有匹配点了，直接跳过
        if(!pMP || vbAlreadyMatched1[i1])//地图点不存在 或 前面SearchByBoW已匹配 ，就跳过
            continue;

        if(pMP->isBad())//地图点是Bad，就跳过
            continue;

        // Step 3.1：通过Sim变换，将pKF1的地图点投影到pKF2中的图像坐标
        cv::Mat p3Dw = pMP->GetWorldPos();//取出当前循环的当前关键帧的特征点索引对应的地图点的世界坐标系下坐标
        // 把pKF1的地图点从world坐标系变换到camera1坐标系
        cv::Mat p3Dc1 = R1w*p3Dw + t1w;//当前关键帧的地图点的世界坐标系坐标转换到当前关键帧的相机坐标系下
        // 再通过Sim3将该地图点从camera1变换到camera2坐标系
        cv::Mat p3Dc2 = sR21*p3Dc1 + t21;//通过sim3变换，把当前关键帧的相机坐标系下地图点 转换到 保留闭环候选关键帧的相机坐标系下  //用到了前面计算的sim3逆矩阵

        if(p3Dc2.at<float>(2)<0.0)//判断变换到保留闭环候选关键帧的相机坐标系下的地图点z深度是否为正值
            continue;

        // 投影到camera2图像坐标 (u,v)
        const float invz = 1.0/p3Dc2.at<float>(2);    //保留闭环候选关键帧的相机坐标系下地图点1/z 
        const float x = p3Dc2.at<float>(0)*invz;      //保留闭环候选关键帧的相机坐标系下地图点x 
        const float y = p3Dc2.at<float>(1)*invz;      //保留闭环候选关键帧的相机坐标系下地图点y

        const float u = fx*x+cx;                      //保留闭环候选关键帧的相机坐标系下地图点 投影到 像素坐标系下
        const float v = fy*y+cy;

        // Point must be inside the image
        if(!pKF2->IsInImage(u,v))//判断u࿰