首页 > 代码库 > ORB-SLAM(六)MapPoint与Map
ORB-SLAM(六)MapPoint与Map
地图点可以通过关键帧来构造,也可以通过普通帧构造,但是最终,必须是和关键帧对应的,通过普通帧构造的地图点只是临时被Tracking用来追踪用的。
构造函数(地图点3D坐标及其参考帧):
// 参考帧是关键帧,该地图点将于许多帧关键帧对应,建立关键帧之间的共视关系 MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap) // 参考帧是普通帧,该地图点只与当前普通帧的特征点对应 MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF)
地图点和关键帧之间的观测关系是最重要的,参考关键帧是哪一帧,该地图点被哪些关键帧观测到,对应的哪个(idx)特征点?通过两个成员维护:
std::map<KeyFrame*,size_t> mObservations; // 观测到该地图点的相机数 int nObs;
添加地图点观测:能够观测到同一个地图点的关键帧之间存在共视关系
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx);
删除地图点观测:从当前地图点的mObservation和nObs成员中删掉对应关键帧观测关系,若该关键帧恰好是参考帧,则需要重新指定。当观测相机数小于等于2时,该地图点需要剔除。删掉观测关系,和删掉地图点(以及替换地图点),需要区分开!
void MapPoint::EraseObservation(KeyFrame* pKF);
剔除MapPoint不仅需要删掉地图点中维护的关键帧观测关系,还需要删掉对应关键中对应的地图点,以及Map中该地图点的内存:
void KeyFrame::EraseMapPointMatch(const size_t &idx) { unique_lock<mutex> lock(mMutexFeatures); mvpMapPoints[idx]=static_cast<MapPoint*>(NULL); } mpMap->EraseMapPoint(this);
下面是一个重要的函数:
void MapPoint::Replace(MapPoint* pMP);
将当前地图点(this),替换成pMp。主要使用在闭环时,调整地图点和关键帧,建立新的关系:
关键帧将联系的this替换成pMap:
pKF->ReplaceMapPointMatch(mit->second, pMP);// KeyFrame中mit->second索引对应的地图点,用pMP替换掉原来的this pMP->AddObservation(pKF,mit->second);// pMp地图点添加观测关键帧
// 删掉Map中该地图点
mpMap->EraseMapPoint(this);
无论是SetBadFlag()还是Replace(),当前地图点的mbBad标志都被记为true,表明当前地图点是个坏点。
visible和found的区别:该地图点在视野范围内,该地图点有对应特征点的帧数。通常来说,found的地图点一定是visible的,但是visible的地图点很可能not found
float MapPoint::GetFoundRatio() { unique_lock<mutex> lock(mMutexFeatures); return static_cast<float>(mnFound)/mnVisible; }
GetFoundRatio低表示该地图点在很多关键帧的视野范围内,但是没有匹配上很多特征点。
最后是MapPoint中几个比较重要的函数:
void MapPoint::ComputeDistinctiveDescriptors(); void MapPoint::UpdateNormalAndDepth(); int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF);
1. 计算地图点描述子:
从mObservations中获取观察到当前地图点的关键帧及对应描述子,描述子放入vDescriptor描述子向量组成的向量中。在这些描述子中,选择距离(类似hamming距离)其他描述子最近的(中值距离最小,看代码去体会一下是什么意思)作为地图点的描述子mDescriptor。
2. 计算地图点平均观测方向和深度
地图点到所有观测到的关键帧相机中心向量,归一化后相加。
深度范围:地图点到参考帧(只有一帧)相机中心距离,乘上参考帧中描述子获取时金字塔放大尺度,得到最大距离mfMaxDistance;最大距离除以整个金字塔最高层的放大尺度得到最小距离mfMinDistance。通常来说,距离较近的地图点,将在金字塔层数较高的地方提取出,距离较远的地图点,在金字塔层数较低的地方提取出(金字塔层数越低,分辨率越高,才能识别出远点)。因此通过地图点的信息(主要是对应描述子),我们可以获得该地图点对应的金字塔层级:
const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
从而预测该地图点在什么距离范围内能够被观测到!
3. int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF)
注意金字塔ScaleFactor和距离的关系:当前特征点对应ScaleFactor为1.2的意思是:图片分辨率下降1.2倍后,可以提取出该特征点(分辨率更高时,肯定也可以提取出,这里取金字塔中能够提取出该特征点最高层级作为该特征点的层级)
同时,由当前特征点的距离,可以推测所在层级。
Map则比较简单,主要负责维护其中关键帧和地图点容器,设置参考地图点用于绘图:
std::set<MapPoint*> mspMapPoints; std::set<KeyFrame*> mspKeyFrames;
ORB-SLAM(六)MapPoint与Map