ORB-SLAM2 从理论到代码实现（十三）：MapPoint 类

MapPoint是地图中的特征点它自身的参数是三维坐标和描述子在这个类中它需要完成的主要工作有以下方面(1) 维护关键帧之间的共视关系(2) 通过计算描述向量之间的距离在多个关键帧的特征点中找最匹配的特征点(3) 在闭环完成修正后需要根据修正的主帧位姿修正特征点(4) 对于非关键帧也产生MapPoint只不过是给Tracking功能临时使用它的主要功能梳理完以后我们就可以看它对应的函数了先通过一张图梳理它的主要函数1. 成员变量// Position in absolute coordinates cv::Mat mWorldPos; // Keyframes observing the point and associated index in keyframe std::mapKeyFrame*,size_t mObservations; // Mean viewing direction cv::Mat mNormalVector; // Best descriptor to fast matching cv::Mat mDescriptor; // Reference KeyFrame KeyFrame* mpRefKF; // Tracking counters记录观测到的帧数每被观测到一次普通帧不要求是关键帧则计数加1 int mnVisible; // 只要在一帧中匹配成功计数就加1 int mnFound; // 不仅要匹配成功经过优化之后还不能被判定为外点 // Bad flag (we do not currently erase MapPoint from memory) bool mbBad; MapPoint* mpReplaced; // Scale invariance distances float mfMinDistance; float mfMaxDistance;2. 构造函数它的构造函数一共有两个分别对应关键帧和普通帧。2.1. 关键帧相关的地图点构造函数MapPoint(const cv::Mat Pos, KeyFrame* pRefKF, Map* pMap)其中Pos指的是该点的3D位置pRefKF是参考关键帧pMap是地图。和关键帧相关的地图点构造函数主要是突出地图点和关键帧之间的观测关系参考关键帧是哪一帧该地图点被哪些关键帧观测到。一个地图点会被多个关键帧观测到多个关键帧之间通过共同观测到地图点而发生的关系叫共视关系在orb slam中就是通过MapPoint类来维护共视关系的。在进行局部BA优化时只优化具有共视关系的这些关键帧其他关键帧的位姿不参与优化。2.2. 普通帧相关的地图点构造函数MapPoint(const cv::Mat Pos, Map* pMap, Frame* pFrame, const int idxF)其中Pos指的是该点的3D位置pMap是地图pFrame是对应的普通帧idxF是地图点在该帧特征点中的索引号。3. AddObservation(KeyFrame* pKF, size_t idx)增加地图点的观测关系它包含两个参数KeyFrame* pKF是对应的关键帧size_t idx是该地图点在关键帧中对应的索引值它的作用是判断此关键帧是否已经在观测关系中了如果是这里就不会添加如果不是往下记录下此关键帧以及此MapPoint的索引就算是记录下观测信息了void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) { unique_lockmutex lock(mMutexFeatures); //如果已经存在观测关系就返回 if(mObservations.count(pKF)) return; //如果不存在就添加 mObservations[pKF]idx; //分成单目和双目两种清空添加观测单目时观测次数加1双目时观测次数加2 if(pKF-mvuRight[idx]0) nObs2; else nObs; }此处涉及到两个重要变量std::mapKeyFrame *, size_t mObservations它是用来存放观测关系的容器把能够观测到该MapPoint的关键帧以及MapPoint在该关键帧中对应的索引值关联并存储起来int nObs它用来记录被观测的次数4. EraseObservation(KeyFrame* pKF)删除观测关系它的参数KeyFrame* pKF指的是关键帧这个函数首先判断该关键帧是否在观测中如果在就从存放观测关系的容器mObservations中移除该关键帧接着判断该帧是否是参考关键帧如果是参考关键帧换成观测的第一帧因为不能没有参考关键帧呀。删除以后如果该MapPoint被观测的次数小于2那么这个MapPoint就没有存在的必要了需要删除。void MapPoint::EraseObservation(KeyFrame* pKF) { bool bBadfalse; { unique_lockmutex lock(mMutexFeatures); //判断该关键帧是否在观测关系中即该关键帧是否看到了这个MapPoint if(mObservations.count(pKF)) { int idx mObservations[pKF]; //这里同样要判断单目和双目单目时观测次数减1双目时减2 if(pKF-mvuRight[idx]0) nObs-2; else nObs--; //删除该关键帧对应的观测关系 mObservations.erase(pKF); //如果关键帧是参考帧则重新指定 if(mpRefKFpKF) mpRefKFmObservations.begin()-first; // 当被观测次数小于等于2时该地图点需要剔除 if(nObs2) bBadtrue; } } //即删除地图点 if(bBad) SetBadFlag(); }5. SetBadFlag删除地图点它的作用就是删除地图点并清除关键帧和地图中所有和该地图点对应的关联关系void MapPoint::SetBadFlag() { mapKeyFrame*,size_t obs; { unique_lockmutex lock1(mMutexFeatures); unique_lockmutex lock2(mMutexPos); mbBadtrue; obs mObservations; //清除该地图点所有的观测关系 mObservations.clear(); } for(mapKeyFrame*,size_t::iterator mitobs.begin(), mendobs.end(); mit!mend; mit) { KeyFrame* pKF mit-first; //删除关键帧中和该MapPoint对应的匹配关系 pKF-EraseMapPointMatch(mit-second); } //从地图中删除MapPoint mpMap-EraseMapPoint(this); }6. Replace(MapPoint* pMP)替换地图点MapPoint* pMP就是用来替换的地图点该函数的作用是将当前地图点 (this)替换成pMp这主要是因为在使用闭环时完成闭环优化以后需要调整地图点和关键帧建立新的关系。具体流程是循环遍历所有的观测信息判断此MapPoint是否在该关键帧中如果在那么只要移除原来MapPoint的匹配信息最后增加这个MapPoint找到的数量以及可见的次数另外地图中要移除原来的那个MapPoint。最后需要计算这个点独有的描述子。void MapPoint::Replace(MapPoint* pMP) { //如果传入的该MapPoint就是当前的MapPoint直接跳出 if(pMP-mnIdthis-mnId) return; int nvisible, nfound; mapKeyFrame*,size_t obs; { unique_lockmutex lock1(mMutexFeatures); unique_lockmutex lock2(mMutexPos); obsmObservations; mObservations.clear(); mbBadtrue; nvisible mnVisible; nfound mnFound; mpReplaced pMP; } for(mapKeyFrame*,size_t::iterator mitobs.begin(), mendobs.end(); mit!mend; mit) { // Replace measurement in keyframe KeyFrame* pKF mit-first; // 如果该MapPoint不在关键帧的观测关系中就添加观测关系 if(!pMP-IsInKeyFrame(pKF)) { pKF-ReplaceMapPointMatch(mit-second, pMP); pMP-AddObservation(pKF,mit-second); } //如果在就删除关键帧和老的MapPoint之间的对应关系 else { pKF-EraseMapPointMatch(mit-second); } } pMP-IncreaseFound(nfound); pMP-IncreaseVisible(nvisible); pMP-ComputeDistinctiveDescriptors(); //删掉Map中该地图点 mpMap-EraseMapPoint(this); }7. ComputeDistinctiveDescriptors计算最匹配的描述子由于一个MapPoint会被许多相机观测到因此在插入关键帧后需要判断是否更新当前点的最适合的描述子。最好的描述子与其他描述子应该具有最小的中值距离因此先获得当前点的所有描述子然后计算描述子之间的两两距离选择每个描述子相对于其它描述子的距离的中值即中值距离这个中值距离最小的描述子作为地图点的描述子。void MapPoint::ComputeDistinctiveDescriptors() { // Retrieve all observed descriptors vectorcv::Mat vDescriptors; mapKeyFrame*,size_t observations; { unique_lockmutex lock1(mMutexFeatures); //如果地图点标记为不好直接返回 if(mbBad) return; observationsmObservations; } //如果观测为空则返回 if(observations.empty()) return; //保留的描述子数最多和观测数一致 vDescriptors.reserve(observations.size()); for(mapKeyFrame*,size_t::iterator mitobservations.begin(), mendobservations.end(); mit!mend; mit) { KeyFrame* pKF mit-first; if(!pKF-isBad()) //针对每帧的对应的都提取其描述子 vDescriptors.push_back(pKF-mDescriptors.row(mit-second)); } if(vDescriptors.empty()) return; // Compute distances between them const size_t N vDescriptors.size(); float Distances[N][N]; for(size_t i0;iN;i) { Distances[i][i]0; for(size_t ji1;jN;j) { int distij ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]); Distances[i][j]distij; Distances[j][i]distij; } } // Take the descriptor with least median distance to the rest // 选择距离其他描述子中值距离最小的描述子作为地图点的描述子基本上类似于取了个均值 int BestMedian INT_MAX; int BestIdx 0; for(size_t i0;iN;i) { vectorint vDists(Distances[i],Distances[i]N); sort(vDists.begin(),vDists.end()); int median vDists[0.5*(N-1)]; if(medianBestMedian) { BestMedian median; BestIdx i; } } { unique_lockmutex lock(mMutexFeatures); mDescriptor vDescriptors[BestIdx].clone(); } }8. UpdateNormalAndDepth更新法向量和深度值由于图像提取描述子是使用金字塔分层提取所以计算法向量和深度可以知道该MapPoint在对应的关键帧的金字塔哪一层可以提取到。明确了目的下一步就是方法问题所谓的法向量就是也就是说相机光心指向地图点的方向计算这个方向方法很简单只需要用地图点的三维坐标减去相机光心的三维坐标就可以。void MapPoint::UpdateNormalAndDepth() { mapKeyFrame*,size_t observations; KeyFrame* pRefKF; cv::Mat Pos; { unique_lockmutex lock1(mMutexFeatures); unique_lockmutex lock2(mMutexPos); if(mbBad) return; observationsmObservations; pRefKFmpRefKF; Pos mWorldPos.clone(); } if(observations.empty()) return; cv::Mat normal cv::Mat::zeros(3,1,CV_32F); int n0; for(mapKeyFrame*,size_t::iterator mitobservations.begin(), mendobservations.end(); mit!mend; mit) { KeyFrame* pKF mit-first; cv::Mat Owi pKF-GetCameraCenter(); //观测点坐标减去关键帧中相机光心的坐标就是观测方向 //也就是说相机光心指向地图点 cv::Mat normali mWorldPos - Owi; //对其进行归一化后相加 normal normal normali/cv::norm(normali); n; } cv::Mat PC Pos - pRefKF-GetCameraCenter(); const float dist cv::norm(PC); const int level pRefKF-mvKeysUn[observations[pRefKF]].octave; const float levelScaleFactor pRefKF-mvScaleFactors[level]; const int nLevels pRefKF-mnScaleLevels; //深度范围地图点到参考帧只有一帧相机中心距离乘上参考帧中描述子获取金字塔放大尺度 //得到最大距离mfMaxDistance;最大距离除以整个金字塔最高层的放大尺度得到最小距离mfMinDistance. //通常说来距离较近的地图点将在金字塔较高的地方提出 //距离较远的地图点在金字塔层数较低的地方提取出金字塔层数越低分辨率越高才能识别出远点 //因此通过地图点的信息主要对应描述子我们可以获得该地图点对应的金字塔层级 //从而预测该地图点在什么范围内能够被观测到 { unique_lockmutex lock3(mMutexPos); mfMaxDistance dist*levelScaleFactor; mfMinDistance mfMaxDistance/pRefKF-mvScaleFactors[nLevels-1]; mNormalVector normal/n; } }9. PredictScale(const float currentDist, KeyFrame* pKF)预测尺度其中currentDist是当前距离pKF是关键帧该函数的作用是预测特征点在金字塔哪一层可以找到。示意图如下注意金字塔ScaleFactor和距离的关系当特征点对应ScaleFactor为1.2的意思是图片分辨率下降1.2倍后可以提取出该特征点分辨率更高的时候肯定也可以提出这里取金字塔中能够提取出该特征点最高层级作为该特征点的层级同时由当前特征点的距离推测所在的层级。int MapPoint::PredictScale(const float currentDist, KeyFrame* pKF) { float ratio; { unique_lockmutex lock(mMutexPos); ratio mfMaxDistance/currentDist; } int nScale ceil(log(ratio)/pKF-mfLogScaleFactor); if(nScale0) nScale 0; else if(nScalepKF-mnScaleLevels) nScale pKF-mnScaleLevels-1; return nScale; }参考文献主要内容来自下文重写了一些描述增加了一些注释ORB SLAM2源码解读(二)MapPoint类 - 知乎