目录

1.函数作用

2. code及解析

3. ORBmatcher::Fuse函数解析（闭环调用版）

---

1.函数作用

        将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，新增或替换当前关键帧组中KF的地图点。

2. code及解析

/**
 * @brief 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，新增或替换当前关键帧组中KF的地图点
 * 因为 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的
 * 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
 * 
 * @param[in] CorrectedPosesMap         矫正的当前KF对应的共视关键帧及Sim3变换
 */
void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap)
{

    // 定义ORB匹配器
    ORBmatcher matcher(0.8);

    // Step 1 遍历待矫正的当前KF的相连关键帧
    for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
    {
        KeyFrame* pKF = mit->first;
        // 矫正过的Sim 变换
        g2o::Sim3 g2oScw = mit->second;
        cv::Mat cvScw = Converter::toCvMat(g2oScw);

        // Step 2 将mvpLoopMapPoints投影到pKF帧匹配，检查地图点冲突并融合
        // mvpLoopMapPoints：与当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点
        vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));
        // vpReplacePoints：存储mvpLoopMapPoints投影到pKF匹配后需要替换掉的新增地图点,索引和mvpLoopMapPoints一致，初始化为空
        // 搜索区域系数为4
        matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);

        // Get Map Mutex
        // 之所以不在上面 Fuse 函数中进行地图点融合更新的原因是需要对地图加锁
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
        const int nLP = mvpLoopMapPoints.size();
        // Step 3 遍历闭环帧组的所有的地图点，替换掉需要替换的地图点
        for(int i=0; i<nLP;i++)
        {
            MapPoint* pRep = vpReplacePoints[i];
            if(pRep)
            {
                // 如果记录了需要替换的地图点
                // 用mvpLoopMapPoints替换掉vpReplacePoints里记录的要替换的地图点
                pRep->Replace(mvpLoopMapPoints[i]);
            }
        }
    }
}

        这里传入的参数@CorrectedPosesMap是当前帧mpCurrentKF的共视关键帧经过修正之后的世界坐标系下的坐标。

        遍历待矫正的当前mpCurrentKF的相连关键帧，取出此帧pKF与此帧在世界坐标系下的坐标cvScw。

        mvpLoopMapPoints是在ComputeSim3函数中计算得到的，其存储的是闭环匹配关键帧和它的共视关键帧的所有地图点。

vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));

        初始化vpReplacePoints，其大小和mvpLoopMapPoints大小相同。

        用Fuse函数将当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点投影到当前关键帧，融合地图点。

matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);

        @pKF：与mpCurrentKF闭环的关键帧的共视关键帧

        @cvScw：世界坐标系到mpCurrentKF闭环的关键帧的共视关键帧的变换矩阵

        @mvpLoopMapPoints：闭环匹配关键帧和它的共视关键帧的所有地图点。

        @4：重投影区域

        @vpReplacePoints：待替换的地图点

        这步结束以后，我们融合了pKF帧和mvpLoopMapPoints中的地图点。对于pKF中没有的地图点直接添加；对于pKF中存在的地图点，我们进行替换操作。

/**
 * @brief 替换地图点，更新观测关系
 * 
 * @param[in] pMP       用该地图点来替换当前地图点
 */
void MapPoint::Replace(MapPoint* pMP)
{
    // 同一个地图点则跳过
    if(pMP->mnId==this->mnId)
        return;

    //要替换当前地图点,有两个工作:
    // 1. 将当前地图点的观测数据等其他数据都"叠加"到新的地图点上
    // 2. 将观测到当前地图点的关键帧的信息进行更新


    // 清除当前地图点的信息，这一段和SetBadFlag函数相同
    int nvisible, nfound;
    map<KeyFrame*,size_t> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        obs=mObservations;
        //清除当前地图点的原有观测
        mObservations.clear();
        //当前的地图点被删除了
        mbBad=true;
        //暂存当前地图点的可视次数和被找到的次数
        nvisible = mnVisible;
        nfound = mnFound;
        //指明当前地图点已经被指定的地图点替换了
        mpReplaced = pMP;
    }

    // 所有能观测到原地图点的关键帧都要复制到替换的地图点上
    //- 将观测到当前地图的的关键帧的信息进行更新
    for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        // Replace measurement in keyframe
        KeyFrame* pKF = mit->first;

        if(!pMP->IsInKeyFrame(pKF))
        {   
            // 该关键帧中没有对"要替换本地图点的地图点"的观测
            pKF->ReplaceMapPointMatch(mit->second, pMP);// 让KeyFrame用pMP替换掉原来的MapPoint
            pMP->AddObservation(pKF,mit->second);// 让MapPoint替换掉对应的KeyFrame
        }
        else
        {
            // 这个关键帧对当前的地图点和"要替换本地图点的地图点"都具有观测
            // 产生冲突，即pKF中有两个特征点a,b（这两个特征点的描述子是近似相同的），这两个特征点对应两个 MapPoint 为this,pMP
            // 然而在fuse的过程中pMP的观测更多，需要替换this，因此保留b与pMP的联系，去掉a与this的联系
            //说白了,既然是让对方的那个地图点来代替当前的地图点,就是说明对方更好,所以删除这个关键帧对当前帧的观测
            pKF->EraseMapPointMatch(mit->second);
        }
    }

    //- 将当前地图点的观测数据等其他数据都"叠加"到新的地图点上
    pMP->IncreaseFound(nfound);
    pMP->IncreaseVisible(nvisible);
    //描述子更新
    pMP->ComputeDistinctiveDescriptors();

    //告知地图,删掉我
    mpMap->EraseMapPoint(this);
}

3. ORBmatcher::Fuse函数解析（闭环调用版）

/**
 * @brief 闭环矫正中使用。将当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点投影到当前关键帧，融合地图点
 * 
 * @param[in] pKF                   当前关键帧
 * @param[in] Scw                   当前关键帧经过闭环Sim3 后的世界到相机坐标系的Sim变换
 * @param[in] vpPoints              与当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点
 * @param[in] th                    搜索范围系数
 * @param[out] vpReplacePoint       替换的地图点
 * @return int                      融合（替换和新增）的地图点数目
 */
int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector<MapPoint *> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint)
{
    // Get Calibration Parameters for later projection
    const float &fx = pKF->fx;
    const float &fy = pKF->fy;
    const float &cx = pKF->cx;
    const float &cy = pKF->cy;

    // Decompose Scw
    // Step 1 将Sim3转化为SE3并分解
    cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3);
    const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));// 计算得到尺度s
    cv::Mat Rcw = sRcw/scw;// 除掉s
    cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw;// 除掉s
    cv::Mat Ow = -Rcw.t()*tcw;

    // Set of MapPoints already found in the KeyFrame
    // 当前帧已有的匹配地图点
    const set<MapPoint*> spAlreadyFound = pKF->GetMapPoints();

    int nFused=0;
    // 与当前帧闭环匹配上的关键帧及其共视关键帧组成的地图点
    const int nPoints = vpPoints.size();

    // For each candidate MapPoint project and match
    // 遍历所有的地图点
    for(int iMP=0; iMP<nPoints; iMP++)
    {
        MapPoint* pMP = vpPoints[iMP];

        // Discard Bad MapPoints and already found
        // 地图点无效 或 已经是该帧的地图点（无需融合），跳过
        if(pMP->isBad() || spAlreadyFound.count(pMP))
            continue;

        // Get 3D Coords.
        // Step 2 地图点变换到当前相机坐标系下
        cv::Mat p3Dw = pMP->GetWorldPos();

        // Transform into Camera Coords.
        cv::Mat p3Dc = Rcw*p3Dw+tcw;

        // Depth must be positive
        if(p3Dc.at<float>(2)<0.0f)
            continue;

        // Project into Image
        // Step 3 得到地图点投影到当前帧的图像坐标
        const float invz = 1.0/p3Dc.at<float>(2);
        const float x = p3Dc.at<float>(0)*invz;
        const float y = p3Dc.at<float>(1)*invz;

        const float u = fx*x+cx;
        const float v = fy*y+cy;

        // Point must be inside the image
        // 投影点必须在图像范围内
        if(!pKF->IsInImage(u,v))
            continue;

        // Depth must be inside the scale pyramid of the image
        // Step 4 根据距离是否在图像合理金字塔尺度范围内和观测角度是否小于60度判断该地图点是否有效
        const float maxDistance = pMP->GetMaxDistanceInvariance();
        const float minDistance = pMP->GetMinDistanceInvariance();
        cv::Mat PO = p3Dw-Ow;
        const float dist3D = cv::norm(PO);

        if(dist3D<minDistance || dist3D>maxDistance)
            continue;

        // Viewing angle must be less than 60 deg
        cv::Mat Pn = pMP->GetNormal();

        if(PO.dot(Pn)<0.5*dist3D)
            continue;

        // Compute predicted scale level
        const int nPredictedLevel = pMP->PredictScale(dist3D,pKF);

        // Search in a radius
        // 计算搜索范围
        const float radius = th*pKF->mvScaleFactors[nPredictedLevel];

        // Step 5 在当前帧内搜索匹配候选点
        const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);

        if(vIndices.empty())
            continue;

        // Match to the most similar keypoint in the radius
        // Step 6 寻找最佳匹配点（没有用到次佳匹配的比例）
        const cv::Mat dMP = pMP->GetDescriptor();

        int bestDist = INT_MAX;
        int bestIdx = -1;
        for(vector<size_t>::const_iterator vit=vIndices.begin(); vit!=vIndices.end(); vit++)
        {
            const size_t idx = *vit;
            const int &kpLevel = pKF->mvKeysUn[idx].octave;

            if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
                continue;

            const cv::Mat &dKF = pKF->mDescriptors.row(idx);

            int dist = DescriptorDistance(dMP,dKF);

            if(dist<bestDist)
            {
                bestDist = dist;
                bestIdx = idx;
            }
        }

        // If there is already a MapPoint replace otherwise add new measurement
        // Step 7 替换或新增地图点
        if(bestDist<=TH_LOW)
        {
            MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
            if(pMPinKF)
            {
                // 如果这个地图点已经存在，则记录要替换信息
                // 这里不能直接替换，原因是需要对地图点加锁后才能替换，否则可能会crash。所以先记录，在加锁后替换
                if(!pMPinKF->isBad())
                    vpReplacePoint[iMP] = pMPinKF;
            }
            else
            {
                // 如果这个地图点不存在，直接添加
                pMP->AddObservation(pKF,bestIdx);
                pKF->AddMapPoint(pMP,bestIdx);
            }
            nFused++;
        }
    }
    // 融合（替换和新增）的地图点数目
    return nFused;
}

        先将Sim3转化为SE3并分解，得到从世界坐标系到帧pKF坐标系的变换Rcw、tcw以及相机光心Ow。

        遍历每一个地图点（pKF和其共视关键帧的地图点）：

        ①地图点无效 或 已经是该帧的地图点（无需融合），跳过

if(pMP->isBad() || spAlreadyFound.count(pMP))

        ②地图点变换到当前相机坐标系下，如果深度小于0则跳过

        ③得到地图点投影到当前帧的图像坐标u，v；即将地图点投影到pKF帧的像素坐标系中。判断投影点是否在有效范围内pKF->IsInImage(u,v)

        ④根据距离是否在图像合理金字塔尺度范围内和观测角度是否小于60度判断该地图点是否有效，无效则跳过

        ⑤若通过这些测试，则将待匹配的地图点的u，v以及搜索区域输入到GetFeaturesInArea函数中，最终返回待匹配地图点在当前帧pKF的匹配索引vIndices。

ORB-SLAM2 ---- Frame::GetFeaturesInArea函数解析https://blog.csdn.net/qq_41694024/article/details/128227154

        ⑥将地图点pMP的描述子取出，与帧pKF中的待匹配地图点对应的特征点的的描述子索引vIndices中每个特征点进行匹配，寻找最佳匹配的地图点在pKF帧中的索引bestIdx以及两特征点最小汉明距离bestDist。

        ⑦如果这个待匹配地图点pMP与当前帧pKF的索引为bestIdx的地图点的匹配汉明距离小于我们设置的阈值TH_LOW，我们认为匹配有效。

        更新观测关系：

        取出与帧pKF中与pMP匹配成功的地图点放在变量pMPinKF中：

        Ⅰ.如果这个地图点已经存在，则记录要替换信息，记录在vpReplacePoint[iMP]（imp是pKF及共视关键帧地图点的索引） = pMPinKF;中。这里不能直接替换，原因是需要对地图点加锁后才能替换，否则可能会crash。所以先记录，在加锁后替换。

        Ⅱ.如果这个地图点不存在，直接添加（说明只有特征点没有地图点）

                pMP->AddObservation(pKF,bestIdx);
                pKF->AddMapPoint(pMP,bestIdx);

        向上层函数返回成功融合的地图点数量nFused。