详谈ORB-SLAM2的地图点MapPoint

ORB-SLAM2中维护的是局部建图，在项目里所谓的地图就是两个数组：特征点数组和关键帧数组。所有关键帧和特征点的结合就是地图信息，所以在ORB-SLAM2中最重要的两个部分就是地图点和关键帧。

这两个部分在设计上非常像，代码重复率很高。

一、特征点和地图点的区别

1、特征点是2D的，相机图像上的点（图像金字塔）

2、地图点是3D的，根据同一特征点在多个图片中的不同位置三角化得到的

地图点在观测到它的帧上必对应某特征点
特征点不一定能够成功三角化出地图点

二、各成员函数/变量

地图点的所有信息：
地图点的世界坐标: mWorldPos、与关键帧的观测信息: mObservations、观测尺度、特征描述子（地图点的特征描述子就是地图点所对应的特征点的描述子）、地图点的删除与替换

1、地图点的世界坐标：mWorldPos

以矩阵的方式存储（cv::Mat），地图点是私有的世界坐标，有两个共有的方法，使用get、set方法目的就是为了加锁，保证访问的一致性。

成员函数/变量	访问控制	意义
cv::Mat mWorldPos	protected	地图点的世界坐标
cv::Mat GetWorldPos()	public	mWorldPos的get方法
void SetWorldPos(const cv::Mat &Pos)	public	mWorldPos的set方法
std::mutex mMutexPos	protected	mWorldPos的锁

2、地图点与关键帧的观测关系mObservations

（1）通过mObservations变量来保存他们之间的观测关系，地图点和关键帧是双向连接的关系

成员变量std::map mObservations保存了当前关键点对关键帧KeyFrame的观测关系，std::map是一个key-value结构，其key为某个关键帧，value为当前地图点在该关键帧中的索引（在该关键帧成员变量std::vector mvpMapPoints中的索引）

（2）mObservations字典操作：

成员函数/变量	访问控制	意义
std::map mObservations	protected	当前地图点在某KeyFrame中的索引
map GetObservations()	public	mObservations的get方法
void AddObservation(KeyFrame* pKF,size_t idx)	public	添加当前地图点对某KeyFrame的观测
void EraseObservation(KeyFrame* pKF)	public	删除当前地图点对某KeyFrame的观测
bool IsInKeyFrame(KeyFrame* pKF)	public	查询当前地图点是否在某KeyFrame中
int GetIndexInKeyFrame(KeyFrame* pKF)	public	查询当前地图点在某KeyFrame中的索引

（3）成员nObs记录了当前地图点被多少个关键帧相机观测到（单目关键帧每次观测算1个相机,双目/RGBD帧每次观测算2个相机）

成员函数/变量	访问控制	意义
int nObs	public	记录当前地图点被多少相机观测到 单目帧每次观测加1,双目帧每次观测加2
int Observations()	public	nObs的get方法

（4）代码实现

函数的增删改查的操作里是同时维护mObservations观测变量和nObs观测变量的统计量，AddObservation()函数和EraseObservation()函数同时维护mObservations和nObs

所有的操作都加锁了，进过普通的字典操作，根据单目或者双目帧，如果变量有右值即右图像就是双目帧，没有则为单目帧（根据有无右图像判断单目帧or双目帧），删除地图点同时加锁操作，之后释放锁

①AddObservation()函数

// 向参考帧pKF中添加对本地图点的观测,本地图点在pKF中的编号为idx
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) {
    unique_lock<mutex> lock(mMutexFeatures);
    // 如果已经添加过观测，返回
    if(mObservations.count(pKF)) 
        return;
    // 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
    mObservations[pKF]=idx;

    // 根据观测形式是单目还是双目更新观测计数变量nObs
    if(pKF->mvuRight[idx]>=0)
        nObs += 2; 
    else
        nObs++; 
}

②EraseObservation()函数

// 从参考帧pKF中移除本地图点
void MapPoint::EraseObservation(KeyFrame* pKF)  {
    bool bBad=false;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        // 查找这个要删除的观测,根据单目和双目类型的不同从其中删除当前地图点的被观测次数
        if(mObservations.count(pKF)) {
            if(pKF->mvuRight[mObservations[pKF]]>=0)
                nObs-=2;
            else
                nObs--;

            mObservations.erase(pKF);

            // 如果该keyFrame是参考帧，该Frame被删除后重新指定RefFrame
            if(mpRefKF == pKF)
                mpRefKF = mObservations.begin()->first;		// ????参考帧指定得这么草率真的好么?

            // 当观测到该点的相机数目少于2时，丢弃该点(至少需要两个观测才能三角化)
            if(nObs<=2)
                bBad=true;
        }
    }

    if(bBad)
        // 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
        SetBadFlag();
}

③GetIndexInKeyFrame()函数对mObservations的简单查询

int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF) {
    unique_lock<mutex> lock(mMutexFeatures);
    if(mObservations.count(pKF))
        return mObservations[pKF];
    else
        return -1;
}

④IsInKeyFrame()函数对mObservations的简单查询

bool MapPoint::IsInKeyFrame(KeyFrame *pKF) {
    unique_lock<mutex> lock(mMutexFeatures);
    return (mObservations.count(pKF));
}

3、观测尺度

成员函数/变量	访问控制	意义
cv::Mat mNormalVector	protected	平均观测方向
float mfMinDistance	protected	平均观测距离的下限
float mfMaxDistance	protected	平均观测距离的上限
cv::Mat GetNormal()	public	mNormalVector的get方法
float GetMinDistanceInvariance()	public	mfMinDistance的get方法
float GetMaxDistanceInvariance()	public	mNormalVector的get方法
void UpdateNormalAndDepth()	public	更新平均观测距离和方向
int PredictScale(const float ¤tDist, KeyFrame* pKF) int PredictScale(const float ¤tDist, Frame* pF)	public public	估计当前地图点在某Frame中对应特征点的金字塔层级
KeyFrame* mpRefKF	protected	当前地图点的参考关键帧
KeyFrame* GetReferenceKeyFrame()	public	mpRefKF的get方法

4、平均观测距离

原理：近大远小。已知绿色点，找到特征点处于图像金字塔第三层，观测距离是dist，如果镜头拉远，那有可能在第7层被找到，特征点会变小，这时候的距离是mfMaxDistance；如果镜头拉近，图像在第0层才能被找到，说明图像相机距离特征点非常近，

公式：
$$\frac{currentDist}{mfMaxDistance}=1.2^{level}$$

$$level=\lceil {\log }_{1.2}(\frac{currentDist}{mfMaxDistance})\rceil$$

当前观测到的地图点与最大观测地图点是1.2个level倍的关系，所以就能计算出第几个level贵观测到的值；如果知道当前观测到的地图点与最大观测地图点的值也可以反求出level。

代码实现：

// pFrame是当前MapPoint的参考帧
const int level = pFrame->mvKeysUn[idxF].octave;
const float levelScaleFactor = pFrame->mvScaleFactors[level];
const int nLevels = pFrame->mnScaleLevels;
mfMaxDistance = dist*levelScaleFactor;                              
mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];    

根据观测到地图点的参考帧，来决定最大和最小的观测距离是多少，在ORBExtractor里面定义的levelScaleFactor变量，nLevels是几个参考帧与顶点差几个Level，就能算出最大距离mfMaxDistance和最小距离mfMinDistance

5、更新平均观测方向和距离: UpdateNormalAndDepth()

函数UpdateNormalAndDepth()更新当前地图点的平均观测方向和距离，其中平均观测方向是根据mObservations中所有观测到本地图点的关键帧取平均得到的；平均观测距离是根据参考关键帧得到的。

void MapPoint::UpdateNormalAndDepth() {
    // step1. 获取地图点相关信息
    map<KeyFrame *, size_t> observations;
    KeyFrame *pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);

        observations = mObservations;
        pRefKF = mpRefKF;            
        Pos = mWorldPos.clone();    
    }

    // step2. 根据观测到但钱地图点的关键帧取平均计算平均观测方向
    cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);
    int n = 0;
    for (KeyFrame *pKF : observations.begin()) {
        normal = normal + normali / cv::norm(mWorldPos - pKF->GetCameraCenter());
        n++;
    }

    // step3. 根据参考帧计算平均观测距离
    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;                      

    {
        unique_lock<mutex> lock3(mMutexPos);
        mfMaxDistance = dist * levelScaleFactor;
        mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
        mNormalVector = normal / n;
    }
}

地图点的平均观测距离是根据其参考关键帧计算的，那么参考关键帧KeyFrame* mpRefKF是如何指定的呢?

• 构造函数中，创建该地图点的参考帧被设为参考关键帧
• 若当前地图点对参考关键帧的观测被删除(EraseObservation(KeyFrame* pKF)),则取第一个观测到当前地图点的关键帧做参考关键帧

函数MapPoint::UpdateNormalAndDepth()的调用时机是：

只要地图点本身或关键帧对该地图点的观测发生变化,就应该调用函数MapPoint::UpdateNormalAndDepth()更新其观测尺度和方向信息

（1）创建地图点时调用UpdateNormalAndDepth()初始化其观测信息

pNewMP->AddObservation(pKF, i);
pKF->AddMapPoint(pNewMP, i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();				// 更新平均观测方向和距离	
mpMap->AddMapPoint(pNewMP);

（2）地图点对关键帧的观测mObservations更新时(跟踪局部地图添加或删除对关键帧的观测时、LocalMapping线程删除冗余关键帧时或LoopClosing线程闭环矫正时),调用UpdateNormalAndDepth()初始化其观测信息

pMP->AddObservation(mpCurrentKeyFrame, i);
pMP->UpdateNormalAndDepth();

（3）地图点世界坐标mWorldPos发生变化时(BA优化之后),调用UpdateNormalAndDepth()初始化其观测信息

pMP->SetWorldPos(cvCorrectedP3Dw);
pMP->UpdateNormalAndDepth();

6、特征描述子

成员函数/变量	访问控制	意义
cv::Mat mDescriptor	protected	当前关键点的特征描述子(所有描述子的中位数)
cv::Mat GetDescriptor()	public	mDescriptor的get方法
void ComputeDistinctiveDescriptors()	public	计算mDescriptor

特征点是2D的，相机图像上的点
地图点是3D的，根据统一特征点在多个图片中的不同位置三角化得到的
地图点是3D点可能会对应到很多帧上，一个地图点可能对应到很多帧上的特征点，实际上是取所有帧上对应的特征点的描述子取中值

圆圈是3D地图点，4帧中都看到了地图点，地图点看到帧之后会对应一个局部的特征点，对应4个特征点，这4个特征点严格上讲都属于当前地图点的描述子，取当前有代表性的描述子，计算中值，因为特征点描述子是一串256位的01向量，两个特征点描述子可以计算距离哪个特征点到其他特征点距离总和最小就取这个距离。

一个地图点在不同关键帧中对应不同的特征点和描述子，其特征描述子mDescriptor是其在所有观测关键帧中描述子的中位数(准确地说，该描述子与其他所有描述子的中值距离最小)

• 特征描述子的更新时机
  一旦某地图点对关键帧的观测mObservations发生改变,就调用函数MapPoint::ComputeDistinctiveDescriptors()更新该地图点的特征描述子
• 特征描述子的用途
  在函数ORBmatcher::SearchByProjection()和ORBmatcher::Fuse()中,通过比较地图点的特征描述子与图片特征点描述，实现将地图点与图像特征点的匹配(3D-2D匹配)

7、地图点的删除与替换

一个地图点在ORB-SLAM里是很重的变量，地图点里面有很多成员变量，要删除地图点是一个很耗时间的事情，然而删除地图点的操作必须是一个加锁的同步操作，因为删除地图点的时候，就不能让其他线程操作使用，所以删除地图点的过程一定要加锁，一方面希望加锁时间更短，另一方面地图点的变量过多可能会导致系统停顿时间过长，建图过程受影响，于是使用先标记后删除的做法

给每个特征点加一个mbBad坏点标记，表示逻辑上被删除，变量虽然没有来得及被删除没有关系，可以后面继续操作，以后用到地图点的时候，都要检查一下mbBad，如果地图点内存里面有这个地方，mbBad设置为true，那么这个点就设置成坏点了，没来及删除而已就不用它了，先标记后清除，所以只用在标记的时候加锁，而当删除成员变量的时候就不用加锁了，等系统空闲出来在删除就可以了

成员函数/变量	访问控制	意义
bool mbBad	protected	坏点标记
bool isBad()	public	查询当前地图点是否被删除(本质上就是查询mbBad)
void SetBadFlag()	public	删除当前地图点
MapPoint* mpReplaced	protected	用来替换当前地图点的新地图点
void Replace(MapPoint *pMP)	public	使用地图点pMP替换当前地图点

（1）地图点的删除: SetBadFlag()

变量mbBad用来表征当前地图点是否被删除

删除地图点的各成员变量是一个较耗时的过程，因此函数SetBadFlag()删除关键点时采取先标记后删除的方式，这里很像操作系统文件管理策略，具体的删除过程分为以下两步

• 默认flase表示好点，先将坏点标记mbBad置为true，逻辑上删除该地图点
• 再依次清空当前地图点的各成员变量，物理上删除该地图点

判断坏点的条件在LocalMapping有专门负责的函数

void MapPoint::SetBadFlag() {
    map<KeyFrame *, size_t> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        mbBad = true;			// 标记mbBad,逻辑上删除当前地图点
        obs = mObservations;
        mObservations.clear();
    }
    
    // 删除关键帧对当前地图点的观测
    for (KeyFrame *pKF : obs.begin()) {
        pKF->EraseMapPointMatch(mit->second);
    }

    // 在地图类上注册删除当前地图点,这里会发生内存泄漏
    mpMap->EraseMapPoint(this);
}

成员变量mbBad表示当前地图点逻辑上是否被删除，在后面用到地图点的地方，都要通过isBad()函数确认当前地图点没有被删除，然后再进行其它操作

KeyFrame的其他类、成员变量用到地图点的时候，首先检查地图点的指针是否为空，在物理上检测，如果指针不为空，mBad这个变量设为true，逻辑上被删除了，如果物理上和逻辑上有一个被删除了，那么这个点就不进行操作

int KeyFrame::TrackedMapPoints(const int &minObs) {
    // ...
    
    
    for (int i = 0; i < N; i++) {
        MapPoint *pMP = mvpMapPoints[i];
        if (pMP && !pMP->isBad()) {			// 依次检查该地图点物理上和逻辑上是否删除,若删除了就不对其操作
            // ...
        }
    }
	
    // ...
}

在这整个函数来说，第一步逻辑删除和第二部物理删除，他们相邻很近
，实际上线逻辑删除后物理删除，只是逻辑删除不需要加锁，可能会和其他线程抢占资源，其他线程不能运行了，物理删除可以和其它线程同时运行，所以只有逻辑删除这一小部分会影响其他线程的运行

（2）地图点的替换: Replace()

将pMP地图点来替换当前地图点，思路与删除地图点差不多，处理旧地图点和新地图点的成员函数之间的冲突，如果变量在两个地图点里都有，这也是很重的操作，mpReplaced就是这样设计的变量表示用来替换当前地图点的新地图点，默认为null，如果设为true， 证明其他地图点不需要找这个变量，找这个指针指向的mpReplaced，mpReplaced把变量替换了，至于具体的Replace操作怎么融合过程，也不用加锁后面也不会被用了，只有删除和替换的线程会用

void MapPoint::Replace(MapPoint *pMP) {
	// 如果是同一地图点则跳过
    if (pMP->mnId == this->mnId)
        return;

	// step1. 逻辑上删除当前地图点
    int nvisible, nfound;
    map<KeyFrame *, size_t> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        obs = mObservations;
        mObservations.clear();
        mbBad = true;
        nvisible = mnVisible;
        nfound = mnFound;
        mpReplaced = pMP;
    }

    // step2. 将当地图点的数据叠加到新地图点上
    for (map<KeyFrame *, size_t>::iterator mit = obs.begin(), mend = obs.end(); mit != mend; mit++) {
        KeyFrame *pKF = mit->first;
        if (!pMP->IsInKeyFrame(pKF)) {
            pKF->ReplaceMapPointMatch(mit->second, pMP);
            pMP->AddObservation(pKF, mit->second);
        } else {
            pKF->EraseMapPointMatch(mit->second);
        }
    }

    pMP->IncreaseFound(nfound);
    pMP->IncreaseVisible(nvisible);
    pMP->ComputeDistinctiveDescriptors();

    // step3. 删除当前地图点
    mpMap->EraseMapPoint(this);
}

三、MapPoint类的用途

1、MapPoint的生命周期

地图点的创建，初始化的时候会创建地图点，当然一个系统可能就初始化一两次，如果顺利就初始化一次，这可不是创建地图点的关键，关键在于Tracking线程，在创建关键帧的时候肯定要创建地图点， 建图就是特征点地图列表和关键帧地图列表

对ORB-SLAM来说

地图 = 征点地图列表 + 关键帧地图列表

所以创建局部建图的时候，就有一部专门创建地图点的设计，但是在跟踪的时候，估计当前位姿，有创建临时地图点，出除了这个函数之后，这个地图点就被删除掉了，创建完临时地图点马上就被删除了，不会注册到系统的全局地图里，这是创建的时机；删除特征点的时机在局部建图的时候有一部分专门删除地图点（Recent MapPoints Culling）还有一个删除关键帧（Local KeyFrames Culling），删除关键帧会连带着删除特征点，替换地图点就一步，这一步就是回环检测，发生闭环之后
开始矫正，把会换前后矫正回来，然后发现有一些特征点是重合的，重合的就根据策略进行替换。

2、MapPoint类三大时机

（1）创建MapPoint的时机

①Tracking线程中初始化过程(Tracking::MonocularInitialization()和Tracking::StereoInitialization())

②Tracking线程中创建新的关键帧(Tracking::CreateNewKeyFrame())

③Tracking线程中恒速运动模型跟踪(Tracking::TrackWithMotionModel())也会产生临时地图点,但这些临时地图点在跟踪成功后会被马上删除(那跟踪失败怎么办?跟踪失败的话不会产生关键帧,这些地图点也不会被注册进地图)

④LocalMapping线程中创建新地图点的步骤(LocalMapping::CreateNewMapPoints())会将当前关键帧与前一关键帧进行匹配,生成新地图点

（2）删除MapPoint的时机

①LocalMapping线程中删除恶劣地图点的步骤(LocalMapping::MapPointCulling()).
删除关键帧的函数KeyFrame::SetBadFlag()会调用函数

②MapPoint::EraseObservation()删除地图点对关键帧的观测,若地图点对关键帧的观测少于2,则地图点无法被三角化,就删除该地图点

（3）替换MapPoint的时机

①LoopClosing线程中闭环矫正(LoopClosing::CorrectLoop())时当前关键帧和闭环关键帧上的地图点发生冲突时，会使用闭环关键帧的地图点替换当前关键帧的地图点

②LoopClosing线程中闭环矫正函数LoopClosing::CorrectLoop()会调用LoopClosing::SearchAndFuse()将闭环关键帧的共视关键帧组中所有地图点投影到当前关键帧的共视关键帧组中，发生冲突时就会替换

此博客参考ncepu_Chen的《5小时让你假装大概看懂ORB-SLAM2源码》