ORB-SLAM2源码逐行解析系列（十六）：ORB-SLAM2特征匹配之地图点融合

1. 地图点融合（替换/新增）

（1）为什么需要进行地图点融合

​ 当地图点规模比较大时，会存在很多邻近的地图点，有的来自跟踪线程，有的来自局部建图线程，有的来自闭环线程，这些邻近的地图点很可能是同一个路标点在不同阶段形成的“影子”。因此，需要将这些地图点融合为一个地图点，这样不仅可以避免冗余，缩小地图点的规模，还能通过融合过程提高地图点的精度。在ORB-SLAM2中，地图点融合主要出现在局部建图线程和闭环线程两个地方。

（2）将局部地图点投影到关键帧中进行融合

融合策略

​ 在ORB-SLAM2的局部建图线程中，将当前关键帧的一级和二级相连关键帧对应的所有地图点投影到当前关键帧中。如果投影的地图点能匹配到当前关键帧的特征点，并且该特征点自身有对应的地图点，那么选择这两个地图点中被观测数目最多的那个来替换这两个地图点。如果投影的地图点能匹配到当前关键帧的特征点，但是该特征点自身没有对应的地图点，那么新增该地图点和关键帧之间的观测关系。

大致流程

• 准备工作：获取当前关键帧pKF位姿、内参、光心在世界坐标系下坐标
• 遍历所有待投影地图点，执行如下操作：
  • 判断地图点的有效性：要求存在、不为Bad，且不是pKF地图点
  • 将地图点通过当前帧位姿投影到当前帧的像素坐标系下，并检查对应的像素点是否在图像范围内
  • 计算当前帧相机光心到地图点的向量，并判断它与地图点的平均观测方向所成的夹角是否小于60°
  • 计算当前帧相机光心到地图点的距离，并以此预测出该地图点对应的特征点所在的金字塔层数
  • 根据上面预测出来的该地图点对应特征点所在的金字塔层数，结合所设阈值th，确定当前帧的搜索半径
  • 根据前面投影出的像素点以及计算出的搜索半径，获取当前帧上的候选匹配点
  • 遍历当前帧上的候选匹配点，若候选匹配点与待投影的地图点不在图像金字塔相邻层，则跳过；若候选匹配点与待投影地图点距离过大，则跳过。否则，获取最小的描述子距离及对应的候选匹配点索引。
  • 若计算出的最小描述子距离小于设定的距离阈值，则将对应的候选匹配点作为最佳匹配点，并且根据最佳匹配点是否存在地图点来融合或新增

代码实现

/**
 * @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;
        // 地图点无效 或 已经是该帧pKF的地图点（无需融合），跳过
        if(pMP->isBad() || pMP->IsInKeyFrame(pKF))
            continue;
        
        // p3Dw: 地图点在世界坐标系下的坐标
        cv::Mat p3Dw = pMP->GetWorldPos();
        // 世界坐标系下的3D点 ==> pKF相机坐标系下的3D点
        cv::Mat p3Dc = Rcw*p3Dw + tcw;
        // 若深度值为负，跳过
        if(p3Dc.at<float>(2)<0.0f)
            continue;
        
        // Step 2 得到地图点投影到关键帧的图像坐标
        // pKF相机坐标系下的3D点 ==> pKF归一化坐标系下的3D点
        const float invz = 1/p3Dc.at<float>(2);
        const float x = p3Dc.at<float>(0)*invz;
        const float y = p3Dc.at<float>(1)*invz;
        // pKF归一化坐标系下的3D点 ==> pKF像素坐标系下的像素点
        const float u = fx*x+cx;
        const float v = fy*y+cy