本次主要讲解的是ORBSLAM2中跟踪线程的最后一步，跟踪局部地图，这里主要是在使用前面三种跟踪方式获取到的粗糙的位姿后使用局部地图来细化当前帧的位姿，这个步骤是在每次进行跟踪时都必须使用的，对应的函数为TrackLocalMap，其中主要进行了以下几个步骤
-
更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
// Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints UpdateLocalMap();函数展开为
void Tracking::UpdateLocalMap() { // This is for visualization // 设置参考地图点用于绘图显示局部地图点（红色） mpMap->SetReferenceMapPoints(mvpLocalMapPoints); // Update // 用共视图来更新局部关键帧和局部地图点 UpdateLocalKeyFrames(); UpdateLocalPoints(); }其中主要是寻找局部关键帧和局部地图点，寻找局部关键帧的函数为
void Tracking::UpdateLocalKeyFrames() { // Each map point vote for the keyframes in which it has been observed // Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧 map<KeyFrame*,int> keyframeCounter; for(int i=0; i<mCurrentFrame.N; i++) { if(mCurrentFrame.mvpMapPoints[i]) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; if(!pMP->isBad()) { // 得到观测到该地图点的关键帧和该地图点在关键帧中的索引 const map<KeyFrame*,size_t> observations = pMP->GetObservations(); // 由于一个地图点可以被多个关键帧观测到,因此对于每一次观测,都对观测到这个地图点的关键帧进行累计投票 for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) // 这里的操作非常精彩！ // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对 // it->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数 // 所以最后keyframeCounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点，也就是共视程度 keyframeCounter[it->first]++; } else { mCurrentFrame.mvpMapPoints[i]=NULL; } } } // 没有当前帧没有共视关键帧，返回 if(keyframeCounter.empty()) return; // 存储具有最多观测次数（max）的关键帧 int max=0; KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL); // Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有3种类型 // 先清空局部关键帧 mvpLocalKeyFrames.clear(); // 先申请3倍内存，不够后面再加 mvpLocalKeyFrames.reserve(3*keyframeCounter.size()); // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points // Step 2.1 类型1：能观测到当前帧地图点的关键帧作为局部关键帧 （将邻居拉拢入伙）（一级共视关键帧） for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++) { KeyFrame* pKF = it->first; // 如果设定为要删除的，跳过 if(pKF->isBad()) continue; // 寻找具有最大观测数目的关键帧 if(it->second>max) { max=it->second; pKFmax=pKF; } // 添加到局部关键帧的列表里 mvpLocalKeyFrames.push_back(it->first); // 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id // 表示它已经是当前帧的局部关键帧了，可以防止重复添加局部关键帧 pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId; } // Include also some not-already-included keyframes that are neighbors to already-included keyframes // Step 2.2 遍历一级共视关键帧，寻找更多的局部关键帧 for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) { // Limit the number of keyframes // 处理的局部关键帧不超过80帧 if(mvpLocalKeyFrames.size()>80) break; KeyFrame* pKF = *itKF; // 类型2:一级共视关键帧的共视（前10个）关键帧，称为二级共视关键帧（将邻居的邻居拉拢入伙） // 如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧 const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10); // vNeighs 是按照共视程度从大到小排列 for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++) { KeyFrame* pNeighKF = *itNeighKF; if(!pNeighKF->isBad()) { // mnTrackReferenceForFrame防止重复添加局部关键帧 if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pNeighKF); pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId; //? 找到一个就直接跳出for循环？ break; } } } // 类型3:将一级共视关键帧的子关键帧作为局部关键帧（将邻居的孩子们拉拢入伙） const set<KeyFrame*> spChilds = pKF->GetChilds(); for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++) { KeyFrame* pChildKF = *sit; if(!pChildKF->isBad()) { if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pChildKF); pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId; //? 找到一个就直接跳出for循环？ break; } } } // 类型3:将一级共视关键帧的父关键帧（将邻居的父母们拉拢入伙） KeyFrame* pParent = pKF->GetParent(); if(pParent) { // mnTrackReferenceForFrame防止重复添加局部关键帧 if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pParent); pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId; //! 感觉是个bug！如果找到父关键帧会直接跳出整个循环 break; } } } // Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧 if(pKFmax) { mpReferenceKF = pKFmax; mCurrentFrame.mpReferenceKF = mpReferenceKF; } }这里主要是将当前帧的局部共视关键帧，局部共视关键帧的共视关键帧，局部共视关键帧的父关键帧和子关键帧加入当前的局部关键帧
寻找局部地图点的函数为
void Tracking::UpdateLocalPoints() { // Step 1：清空局部地图点 mvpLocalMapPoints.clear(); // Step 2：遍历局部关键帧 mvpLocalKeyFrames for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) { KeyFrame* pKF = *itKF; const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches(); // step 2：将局部关键帧的地图点添加到mvpLocalMapPoints for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++) { MapPoint* pMP = *itMP; if(!pMP) continue; // 用该地图点的成员变量mnTrackReferenceForFrame 记录当前帧的id // 表示它已经是当前帧的局部地图点了，可以防止重复添加局部地图点 if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId) continue; if(!pMP->isBad()) { mvpLocalMapPoints.push_back(pMP); pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId; } } } }主要是将前面获得的局部关键帧中对应的地图点加入到局部地图点
-
筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系
// Step 2：筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系 SearchLocalPoints();函数展开为
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); } } -
进行纯位姿BA，以获得更加精细的位姿估计
// Step 3：前面新增了更多的匹配关系，BA优化得到更准确的位姿 Optimizer::PoseOptimization(&mCurrentFrame); mnMatchesInliers = 0; -
更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目
// Step 4：更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目 for(int i=0; i<mCurrentFrame.N; i++) { if(mCurrentFrame.mvpMapPoints[i]) { // 由于当前帧的地图点可以被当前帧观测到，其被观测统计量加1 if(!mCurrentFrame.mvbOutlier[i]) { // 找到该点的帧数mnFound 加 1 mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); //查看当前是否是在纯定位过程 if(!mbOnlyTracking) { // 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1 // nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2 if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) mnMatchesInliers++; } else // 记录当前帧跟踪到的地图点数目，用于统计跟踪效果 mnMatchesInliers++; } // 如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点 // ?单目就不管吗 else if(mSensor==System::STEREO) mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL); } } -
根据跟踪匹配数目及重定位情况决定是否跟踪成功
// Step 5：根据跟踪匹配数目及重定位情况决定是否跟踪成功 // 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪 if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50) return false; //如果是正常的状态话只要跟踪的地图点大于30个就认为成功了 if(mnMatchesInliers<30) return false; else return true;
225
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
