目录

1.函数作用

2.code 

3.函数解析 

1.函数作用

        将参数一的关键帧的地图点与参数二的地图点集合进行融合。

        将地图点投影到关键帧中进行匹配和融合；融合策略如下：
        1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
        2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点

2.code 

/**
 * @brief 将地图点投影到关键帧中进行匹配和融合；融合策略如下
 * 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
 * 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点

 * @param[in] pKF           关键帧
 * @param[in] vpMapPoints   待投影的地图点
 * @param[in] th            搜索窗口的阈值，默认为3
 * @return int              更新地图点的数量
 */
int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)
{
    // 取出当前帧位姿、内参、光心在世界坐标系下坐标
    cv::Mat Rcw = pKF->GetRotation();
    cv::Mat tcw = pKF->GetTranslation();

    const float &fx = pKF->fx;
    const float &fy = pKF->fy;
    const float &cx = pKF->cx;
    const float &cy = pKF->cy;
    const float &bf = pKF->mbf;

    cv::Mat Ow = pKF->GetCameraCenter();

    int nFused=0;

    const int nMPs = vpMapPoints.size();

    // 遍历所有的待投影地图点
    for(int i=0; i<nMPs; i++)
    {
        
        MapPoint* pMP = vpMapPoints[i];
        // Step 1 判断地图点的有效性 
        if(!pMP)
            continue;
        // 地图点无效 或 已经是该帧的地图点（无需融合），跳过
        if(pMP->isBad() || pMP->IsInKeyFrame(pKF))
            continue;

        // 将地图点变换到关键帧的相机坐标系下
        cv::Mat p3Dw = pMP->GetWorldPos();
        cv::Mat p3Dc = Rcw*p3Dw + tcw;

        // Depth must be positive
        // 深度值为负，跳过
        if(p3Dc.at<float>(2)<0.0f)
            continue;

        // Step 2 得到地图点投影到关键帧的图像坐标
        const float invz = 1/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;

        const float ur = u-bf*invz;

        const float maxDistance = pMP->GetMaxDistanceInvariance();
        const float minDistance = pMP->GetMinDistanceInvariance();
        cv::Mat PO = p3Dw-Ow;
        const float dist3D = cv::norm(PO);

        // Depth must be inside the scale pyramid of the image
        // Step 3 地图点到关键帧相机光心距离需满足在有效范围内
        if(dist3D<minDistance || dist3D>maxDistance )
            continue;

        // Viewing angle must be less than 60 deg
        // Step 4 地图点到光心的连线与该地图点的平均观测向量之间夹角要小于60°
        cv::Mat Pn = pMP->GetNormal();
        if(PO.dot(Pn)<0.5*dist3D)
            continue;
        // 根据地图点到相机光心距离预测匹配点所在的金字塔尺度
        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 = 256;
        int bestIdx = -1;
        for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)// 步骤3：遍历搜索范围内的features
        {
            const size_t idx = *vit;

            const cv::KeyPoint &kp = pKF->mvKeysUn[idx];

            const int &kpLevel= kp.octave;
            // 金字塔层级要接近（同一层或小一层），否则跳过
            if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
                continue;

            // 计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过
            if(pKF->mvuRight[idx]>=0)
            {
                // Check reprojection error in stereo
                // 双目情况
                const float &kpx = kp.pt.x;
                const float &kpy = kp.pt.y;
                const float &kpr = pKF->mvuRight[idx];
                const float ex = u-kpx;
                const float ey = v-kpy;
                // 右目数据的偏差也要考虑进去
                const float er = ur-kpr;        
                const float e2 = ex*ex+ey*ey+er*er;

                //自由度为3, 误差小于1个像素,这种事情95%发生的概率对应卡方检验阈值为7.82
                if(e2*pKF->mvInvLevelSigma2[kpLevel]>7.8)   
                    continue;
            }
            else
            {
                // 计算投影点与候选匹配特征点的距离，如果偏差很大，直接跳过
                // 单目情况
                const float &kpx = kp.pt.x;
                const float &kpy = kp.pt.y;
                const float ex = u-kpx;
                const float ey = v-kpy;
                const float e2 = ex*ex+ey*ey;

                // 自由度为2的，卡方检验阈值5.99（假设测量有一个像素的偏差）
                if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99)
                    continue;
            }

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

            const 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)
            {
                // 如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换
                if(!pMPinKF->isBad())
                {
                    if(pMPinKF->Observations()>pMP->Observations())
                        pMP->Replace(pMPinKF);
                    else
                        pMPinKF->Replace(pMP);
                }
            }
            else
            {
                // 如果最佳匹配点没有对应地图点，添加观测信息
                pMP->AddObservation(pKF,bestIdx);
                pKF->AddMapPoint(pMP,bestIdx);
            }
            nFused++;
        }
    }

    return nFused;
}

3.函数解析 

        我们最终目的是将参数一的关键帧（待融合关键帧pKF）的地图点与参数二的地图点（待匹配地图点vpMapPoints）集合进行融合。

        获取参数一的关键帧（待融合关键帧pKF）位姿、内参、光心在世界坐标系下坐标方便下面使用：Rcw、tcw、fx、fy、cx、cy、bf、Ow。

        将本函数成功融合的地图点nFused值初始化设置为0。

        遍历所有的待投影地图点：

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

        ②将地图点变换到关键帧pKF的相机坐标系下，深度值为负，跳过。

        ③将地图点投影到关键帧pKF的像素坐标系下，若超出了像素坐标系，跳过。

        ④地图点到关键帧相机光心距离需满足在有效范围内，不满足，跳过。

        ⑤地图点到光心的连线与该地图点的平均观测向量之间夹角要小于60°，不满足跳过。

        若上述条件都满足，我们利用重投影的方法获得待匹配地图点在关键帧中的待匹配地图点的索引vIndices。也就是说，这个地图点只可能和关键帧pKF中的索引为vIndices的地图点相融合。

        接下来遍历寻找最佳的匹配点，步骤如下：

        ①获取当前待融合地图点的描述子dMP。

        ②计算当前帧（待融合关键帧）pKF的筛选好的特征点（其索引为vIndices）和当前遍历待融合地图点的描述子的汉明距离dMP。并进行筛选，最后我们记录和投影点的描述子距离最小的pKF帧的最小汉明距离bestDist和特征点索引bestIdx。

        ③如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换

            if(pMPinKF)
            {
                // 如果最佳匹配点有对应有效地图点，选择被观测次数最多的那个替换
                if(!pMPinKF->isBad())
                {
                    if(pMPinKF->Observations()>pMP->Observations())
                        pMP->Replace(pMPinKF);
                    else
                        pMPinKF->Replace(pMP);
                }
            }

        ④如果最佳匹配点没有对应地图点，添加观测信息。

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

        更新地图点的操作具体如下：

ORB-SLAM2 --- MapPoint::Replace函数解析https://blog.csdn.net/qq_41694024/article/details/128565834        返回成功融合的地图点数目。