炫酷手机网站模板,网站百度收录查询,东莞公司注册地址查询,做网站赚钱好难文章目录 一、函数意义二、函数讲解三、函数代码四、本函数使用的匹配方法ORBmatcher::Fuse()1. 函数讲解2. 函数代码 四、总结 一、函数意义 本函数是用于地图点融合的函数#xff0c;前面的函数生成了新的地图点#xff0c;但这些地图点可能在前面的关键帧中已经生成过了前面的函数生成了新的地图点但这些地图点可能在前面的关键帧中已经生成过了这时候就需要判断是否重复生成如果重复生成就只保留最观测观测次数最多的那一个关键帧的地图点地图点融合。 二、函数讲解 想要融合地图点们就要找出那些关键帧与当前关键帧有共视关系。本函数使用的方法是找出与当前关键帧公式关系最好的10个关键帧单目为20个作为一级共视关键帧然后找出一级共视关键帧共视关系最好的5个关键帧作为二级共视关键帧。接下来的融合将从一级和二级共视关键帧进行。本函数为了保证融合的完全性和正确性采用了正反两次融合第一次将当前关键帧的点投影到共视关键帧中寻求匹配和融合。第二次将共视关键帧中的地图点投影到当前关键帧中寻求匹配和融合。这两次特征匹配使用的方法都是半径搜索法相对于词袋匹配的方法此方法是精匹配。 三、函数代码
// 检查并融合当前关键帧与相邻帧两级相邻重复的地图点
void LocalMapping::SearchInNeighbors()
{// Retrieve neighbor keyframes// 单目情况要20个邻接关键帧双目或者RGBD则要10个int nn 10;if(mbMonocular)nn20;// 获取与该关键帧连接的前N个最强共视关键帧(已按权值排序)const vectorKeyFrame* vpNeighKFs mpCurrentKeyFrame-GetBestCovisibilityKeyFrames(nn);// 存储一级相邻关键帧及其二级相邻关键帧vectorKeyFrame* vpTargetKFs;// 遍历这些共视的关键帧for(vectorKeyFrame*::const_iterator vitvpNeighKFs.begin(), vendvpNeighKFs.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 vectorKeyFrame* vpSecondNeighKFs pKFi-GetBestCovisibilityKeyFrames(5);// 同样的操作获取二级相邻关键帧for(vectorKeyFrame*::const_iterator vit2vpSecondNeighKFs.begin(), vend2vpSecondNeighKFs.end(); vit2!vend2; vit2){KeyFrame* pKFi2 *vit2;if(pKFi2-isBad() || pKFi2-mnFuseTargetForKFmpCurrentKeyFrame-mnId || pKFi2-mnIdmpCurrentKeyFrame-mnId)continue;vpTargetKFs.push_back(pKFi2);}}// Search matches by projection from current KF in target KFs// 将当前帧的地图点分别投影到两级相邻关键帧寻找匹配点对应的地图点进行融合称为正向投影融合// 初始化特征匹配器ORBmatcher matcher;// 获取当前帧匹配的地图点vectorMapPoint* vpMapPointMatches mpCurrentKeyFrame-GetMapPointMatches();// 遍历一级和二级的相邻关键帧融合地图点for(vectorKeyFrame*::iterator vitvpTargetKFs.begin(), vendvpTargetKFs.end(); vit!vend; vit){// 获取关键帧的指针KeyFrame* pKFi *vit;// 将地图点投影到关键帧中进行匹配和融合融合策略如下// 1.如果地图点能匹配关键帧的特征点并且该点有对应的地图点那么选择观测数目多的替换两个地图点// 2.如果地图点能匹配关键帧的特征点并且该点没有对应的地图点那么为该点添加该投影地图点// 注意这个时候对地图点融合的操作是立即生效的 matcher.Fuse(pKFi,vpMapPointMatches);}// Search matches by projection from target KFs in current KF// 反向匹配vectorMapPoint* vpFuseCandidates;vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());// 遍历一级和二级的相邻关键帧// 查找当前关键帧中的候选地图点查看它们是否与相邻关键帧中已存在的地图点匹配。// 如果匹配则将它们加入到待融合的候选列表 vpFuseCandidates 中for(vectorKeyFrame*::iterator vitKFvpTargetKFs.begin(), vendKFvpTargetKFs.end(); vitKF!vendKF; vitKF){// 获取指针KeyFrame* pKFi *vitKF;// 获取匹配的地图点vectorMapPoint* vpMapPointsKFi pKFi-GetMapPointMatches();// 遍历这些地图点for(vectorMapPoint*::iterator vitMPvpMapPointsKFi.begin(), vendMPvpMapPointsKFi.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 i0, iendvpMapPointMatches.size(); iiend; i){MapPoint* pMPvpMapPointMatches[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 vectorMapPoint * 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 nFused0;// 获取待融合的地图点的数量const int nMPs vpMapPoints.size();// 遍历待融合的地图点for(int i0; inMPs; 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.atfloat(2)0.0f)continue;// 计算图像坐标const float invz 1/p3Dc.atfloat(2);const float x p3Dc.atfloat(0)*invz;const float y p3Dc.atfloat(1)*invz;const float u fx*xcx;const float v fy*ycy;// 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(dist3DminDistance || dist3DmaxDistance )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 vectorsize_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(vectorsize_t::const_iterator vitvIndices.begin(), vendvIndices.end(); vit!vend; vit){// 获取指针const size_t idx *vit;// 获取去畸变后的坐标const cv::KeyPoint kp pKF-mvKeysUn[idx];// 获取当前金字塔层级const int kpLevel kp.octave;// 该点层级大于预测层级或小于两个层级就返回if(kpLevelnPredictedLevel-1 || kpLevelnPredictedLevel)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*exey*eyer*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*exey*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(distbestDist){bestDist dist;bestIdx idx;}}// If there is already a MapPoint replace otherwise add new measurement// 判断最佳距离是否在阈值内if(bestDistTH_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;
}
四、总结 本函数是为创早的新地图点服务的函数目的是减少重复的地图点数量同时优化地图点的坐标选择观测最多的那个帧对应的地图点保留本身就是一种优化我们可以看到进入到局部见图线程以后不再使用普通真而全部使用关键帧这是因为关键帧是经过筛选的相对一一般的帧更好而且这样做的好处还有减少计算量只用关键帧可以大大减少匹配次数和难度在优化时作用更加明显显著提高效。
