1.函数作用
用局部地图点进行投影匹配,得到更多的匹配关系。
局部地图点中已经是当前帧地图点的不需要再投影,只需要将此外的并且在视野范围内的点和当前帧进行投影匹配。
2.函数流程
Step 1:遍历当前帧的地图点,标记这些地图点不参与之后的投影搜索匹配
Step 2:判断所有局部地图点中除当前帧地图点外的点,是否在当前帧视野范围内
Step 3:如果需要进行投影匹配的点的数目大于0,就进行投影匹配,增加更多的匹配关系
3.code
void Tracking::SearchLocalPoints() { // Do not search map points already matched // Step 1:遍历当前帧的地图点,标记这些地图点不参与之后的投影搜索匹配 for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++) { MapPoint* pMP = *vit; if(pMP) { if(pMP->isBad()) { *vit = static_cast<MapPoint*>(NULL); } else { // 更新能观测到该点的帧数加1(被当前帧观测了) pMP->IncreaseVisible(); // 标记该点被当前帧观测到 pMP->mnLastFrameSeen = mCurrentFrame.mnId; // 标记该点在后面搜索匹配时不被投影,因为已经有匹配了 pMP->mbTrackInView = false; } } } // 准备进行投影匹配的点的数目 int nToMatch=0; // Project points in frame and check its visibility // Step 2:判断所有局部地图点中除当前帧地图点外的点,是否在当前帧视野范围内 for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++) { MapPoint* pMP = *vit; // 已经被当前帧观测到的地图点肯定在视野范围内,跳过 if(pMP->mnLastFrameSeen == mCurrentFrame.mnId) continue; // 跳过坏点 if(pMP->isBad()) continue; // Project (this fills MapPoint variables for matching) // 判断地图点是否在在当前帧视野内 if(mCurrentFrame.isInFrustum(pMP,0.5)) { // 观测到该点的帧数加1 pMP->IncreaseVisible(); // 只有在视野范围内的地图点才参与之后的投影匹配 nToMatch++; } } // Step 3:如果需要进行投影匹配的点的数目大于0,就进行投影匹配,增加更多的匹配关系 if(nToMatch>0) { ORBmatcher matcher(0.8); int th = 1; if(mSensor==System::RGBD) //RGBD相机输入的时候,搜索的阈值会变得稍微大一些 th=3; // If the camera has been relocalised recently, perform a coarser search // 如果不久前进行过重定位,那么进行一个更加宽泛的搜索,阈值需要增大 if(mCurrentFrame.mnId<mnLastRelocFrameId+2) th=5; // 投影匹配得到更多的匹配关系 matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th); } }
4.函数解析
一、遍历当前帧的地图点,对于是坏点的地图点将其删除。若该地图点不是坏点,则
①该地图点能被观测到的帧数mnVisible加1
②通过mnLastFrameSeen标记位设置为当前帧从而得到该点被当前帧观测到的标记。
③标记该点在后面搜索匹配时不被投影的标记mbTrackInView,因为已经有匹配了。
二、判断所有局部地图点mvpLocalMapPoints中除当前帧地图点外的点,是否在当前帧视野范围内:
注意:局部地图点在Tracking::UpdateLocalPoints函数中得到
ORB-SLAM2 --- Tracking::UpdateLocalPoints函数解析https://blog.csdn.net/qq_41694024/article/details/128321282
①若已经被当前帧观测到的地图点肯定在视野范围内,跳过。(通过mnLastFrameSeen标记判断是否该地图点被当前帧观测)
②跳过坏点
③判断地图点是否在在当前帧视野内,若该地图点在当前帧的视野中,将观测到该点的帧数加1。(因为我们是一帧帧传进来的,不存在重复计数)
ORB-SLAM2 --- Frame::isInFrustum函数解析https://blog.csdn.net/qq_41694024/article/details/128334862
三、如果需要进行投影匹配的点的数目nToMatch大于0,就进行投影匹配,增加更多的匹配关系
如果不久前进行过重定位,那么进行一个更加宽泛的搜索,阈值需要增大。
我们利用投影匹配法找到更多的匹配关系。
ORB-SLAM2 --- ORBmatcher::SearchByProjection函数解析https://mp.csdn.net/mp_blog/creation/editor/128365489