ORB-SLAM2 ----- LocalMapping::SearchInNeighbors()

一、函数意义

本函数是用于地图点融合的函数，前面的函数生成了新的地图点，但这些地图点可能在前面的关键帧中已经生成过了，这时候就需要判断是否重复生成，如果重复生成就只保留最观测观测次数最多的那一个关键帧的地图点（地图点融合）。

二、函数讲解

想要融合地图点们就要找出那些关键帧，与当前关键帧有共视关系。本函数使用的方法是，找出与当前关键帧公式关系最好的10个关键帧（单目为20个）作为一级共视关键帧，然后找出一级共视关键帧共视关系最好的5个关键帧作为二级共视关键帧。接下来的融合将从一级和二级共视关键帧进行。本函数为了保证融合的完全性和正确性，采用了正反两次融合，第一次将当前关键帧的点投影到共视关键帧中，寻求匹配和融合。第二次将共视关键帧中的地图点投影到当前关键帧中，寻求匹配和融合。这两次特征匹配使用的方法都是，半径搜索法，相对于词袋匹配的方法，此方法是精匹配。

三、函数代码

// 检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点
void LocalMapping::SearchInNeighbors()
{// Retrieve neighbor keyframes// 单目情况要20个邻接关键帧，双目或者RGBD则要10个int nn = 10;if(mbMonocular)nn=20;// 获取与该关键帧连接的前N个最强共视关键帧(已按权值排序)const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);// 存储一级相邻关键帧及其二级相邻关键帧vector<KeyFrame*> vpTargetKFs;// 遍历这些共视的关键帧for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++){// 获取关键帧的指针KeyFrame* pKFi = *vit;// 排除坏点和已经融合的点if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)continue;// 获取一级相邻关键帧vpTargetKFs.push_back(pKFi);// 获取索引pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// Extend to some second neighbors// 以一级相邻关键帧的共视关系最好的5个相邻关键帧 作为二级相邻关键帧const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);// 同样的操作，获取二级相邻关键帧for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++){KeyFrame* pKFi2 = *vit2;if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)continue;vpTargetKFs.push_back(pKFi2);}}// Search matches by projection from current KF in target KFs// 将当前帧的地图点分别投影到两级相邻关键帧，寻找匹配点对应的地图点进行融合，称为正向投影融合// 初始化特征匹配器ORBmatcher matcher;// 获取当前帧匹配的地图点vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();// 遍历一级和二级的相邻关键帧，融合地图点for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++){// 获取关键帧的指针KeyFrame* pKFi = *vit;// 将地图点投影到关键帧中进行匹配和融合；融合策略如下// 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点// 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点// 注意这个时候对地图点融合的操作是立即生效的 matcher.Fuse(pKFi,vpMapPointMatches);}// Search matches by projection from target KFs in current KF// 反向匹配vector<MapPoint*> vpFuseCandidates;vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());// 遍历一级和二级的相邻关键帧，// 查找当前关键帧中的候选地图点，查看它们是否与相邻关键帧中已存在的地图点匹配。// 如果匹配，则将它们加入到待融合的候选列表 vpFuseCandidates 中for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++){// 获取指针KeyFrame* pKFi = *vitKF;// 获取匹配的地图点vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();// 遍历这些地图点for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++){   // 获取指针MapPoint* pMP = *vitMP;// 排除不合格的点if(!pMP)continue;if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)continue;// 获取索引pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;// 将它们加入到待融合的候选列表 vpFuseCandidates 中vpFuseCandidates.push_back(pMP);}}// 进一步融合matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);// Update points// 获取本关键帧的地图点vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();// 遍历这些地图点for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++){MapPoint* pMP=vpMapPointMatches[i];if(pMP){if(!pMP->isBad()){// 计算该地图点最具代表性的描述子pMP->ComputeDistinctiveDescriptors();// 获取其深度和法向量pMP->UpdateNormalAndDepth();}}}// Update connections in covisibility graph// 更新连接关系mpCurrentKeyFrame->UpdateConnections();
}

四、本函数使用的匹配方法ORBmatcher::Fuse()

1. 函数讲解

ORBmatcher::Fuse()函数是这个函数中调用的特征匹配的方法，本函数是一个有返回值的函数，返回值为匹配融合成功的地图点的个数。注意到函数中有两层for循环，这两个函数构成了函数的主题，第一层是遍历候选的地图点（这些地图点来源于主函数筛选出来的，准备融合的地图点），第二层是将地图点投影到别的帧，获取那个帧以该投影点为中心，以r为半径的圆内的点，找出这些点中最匹配的那个点，作为匹配成功的点，但如果最匹配的点也超出了阈值，就判定为该地图点匹配失败。匹配成功后筛选不合格的点，然后为最终融合成功的点增加观测。该函数的融合原则为如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点，如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点。正反两次特征匹配使用的相同函数，只是函数的参数不同，即投影的方向不同。

2. 函数代码

/*** @brief 将地图点投影到关键帧中进行匹配和融合* @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];// 排除不符合规定的点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;// 计算图像坐标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// 检查距离是否在有效范围内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;// 预测金字塔层级并查找特征点int nPredictedLevel = pMP->PredictScale(dist3D,pKF);// Search in a radius// 设置搜索半径const float radius = th*pKF->mvScaleFactors[nPredictedLevel];// 获取以该特征点为中心，以radius为半径搜索获取特征点const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);// 没有获取到点就返回if(vIndices.empty())continue;// Match to the most similar keypoint in the radius// 获取描述子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++){// 获取指针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 stereoconst 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;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;// 第一个点的尺度为5.99if(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// 判断最佳距离是否在阈值内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);}// 增加成功融合计数 nFusednFused++;}}return nFused;
}

四、总结

本函数是为创早的新地图点服务的函数，目的是减少重复的地图点数量，同时优化地图点的坐标（选择观测最多的那个帧对应的地图点保留，本身就是一种优化），我们可以看到，进入到局部见图线程以后，不再使用普通真，而全部使用关键帧，这是因为关键帧是经过筛选的，相对一一般的帧更好，而且这样做的好处还有减少计算量，只用关键帧，可以大大减少匹配次数和难度，在优化时作用更加明显，显著提高效。