ORB-SLAM2源码学习：ORBmatcher.cc⑥: int ORBmatcher::Fuse将地图点投影到关键帧中进行匹配和融合

前言

该函数定义在特征匹配的源文件（ORBmatcher.cc）下，用于将地图点投影到关键帧中进行匹配和融合。

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

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

ORB-SLAM2源码学习：LocalMapping.cc: void LocalMapping::SearchInNeighbors 检查并融合当前关键帧和相邻帧（两级相邻）的重复地图点-CSDN博客

该函数在局部建图线程中的检查并融合相邻帧的地图点时被调用进行正反两个方向的融合。

在闭环线程中，我们将当前关键帧闭环匹配上的关键帧及其共视关键帧组成的所有地图点投影到当前关键帧中，然后执行上述地图点融合（替换或新增）操作。

1.函数声明

int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)

融合方式：

1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换那个地图点

2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点

2.函数定义

具体流程如下：

1.取出当前帧位姿、内参、光心在世界坐标系下的坐标。

2.开始遍历所有地图点，判断投影地图点是否有效。如果地图点不存在、无效、已经在当前帧中（无须融合），则跳过该地图点，遍历下一个地图点。

3.对于有效的地图点，将其投影到当前关键帧中的二维图像坐标。

4.投影坐标需要在有效的图像范围内，地图点到关键帧相机光心的距离需满足在有效范围内，地图点到光心的连线与该地图点的平均观测方向向量之间的夹角要小于60° 。同时满足这些条件后继续下一步，否则跳过该地图点，遍历下一个地图点。

5.根据地图点到相机光心的距离预测匹配点所在的金字塔尺度，确定搜索范围，确定候选匹配点。

6.遍历候选匹配点，最佳候选匹配点要同时满足投影点和候选匹配点的距离在合理范围内（因此此时的位姿认为是准确的，投影位置也接近真实匹配点），以及投影点的描述子距离最小。

7.根据匹配点对应的地图点情况执行地图点替换或新增。

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.82if(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);elsepMPinKF->Replace(pMP);}}else{// 如果最佳匹配点没有对应地图点，添加观测信息pMP->AddObservation(pKF,bestIdx);pKF->AddMapPoint(pMP,bestIdx);}nFused++;}}return nFused;
}