ORB-SLAM2 --- MapPoint::Replace函数

目录

1.函数作用

2.code 

3.函数解析 

---

1.函数作用

        替换地图点，更新观测关系。

2.code 

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

    //要替换当前地图点,有两个工作:
    // 1. 将当前地图点的观测数据等其他数据都"叠加"到新的地图点上
    // 2. 将观测到当前地图点的关键帧的信息进行更新


    // 清除当前地图点的信息，这一段和SetBadFlag函数相同
    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;
    }

    // 所有能观测到原地图点的关键帧都要复制到替换的地图点上
    //- 将观测到当前地图的的关键帧的信息进行更新
    for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        // Replace measurement in keyframe
        KeyFrame* pKF = mit->first;

        if(!pMP->IsInKeyFrame(pKF))
        {   
            // 该关键帧中没有对"要替换本地图点的地图点"的观测
            pKF->ReplaceMapPointMatch(mit->second, pMP);// 让KeyFrame用pMP替换掉原来的MapPoint
            pMP->AddObservation(pKF,mit->second);// 让MapPoint替换掉对应的KeyFrame
        }
        else
        {
            // 这个关键帧对当前的地图点和"要替换本地图点的地图点"都具有观测
            // 产生冲突，即pKF中有两个特征点a,b（这两个特征点的描述子是近似相同的），这两个特征点对应两个 MapPoint 为this,pMP
            // 然而在fuse的过程中pMP的观测更多，需要替换this，因此保留b与pMP的联系，去掉a与this的联系
            //说白了,既然是让对方的那个地图点来代替当前的地图点,就是说明对方更好,所以删除这个关键帧对当前帧的观测
            pKF->EraseMapPointMatch(mit->second);
        }
    }

    //- 将当前地图点的观测数据等其他数据都"叠加"到新的地图点上
    pMP->IncreaseFound(nfound);
    pMP->IncreaseVisible(nvisible);
    //描述子更新
    pMP->ComputeDistinctiveDescriptors();

    //告知地图,删掉我
    mpMap->EraseMapPoint(this);
}

3.函数解析 

        函数的调用形式为pMPinKF->Replace(pMP)。将pMPinKF地图点替换成pMP。

        这里this变量代表着pMPinKF，是调用这个函数的地图点。若两个地图点的ID一样，则表示调用出错误了，我们退出函数。

        mObservations变量是map<KeyFrame*,size_t>型变量，存储着pMPinKF地图点被哪些关键帧观测到，该地图点/特征点在该关键帧的索引信息。

        由于我们要将pMPinKF地图点替换成pMP，因此我们要把地图点pMPinKF的信息清空并将地图点的信息赋值给地图点pMP。

        因此我们要清空pMPinKF地图点的mObservations变量，并用obs变量存储原有的信息方便复制到pMP地图点。将地图点pMPinKF的mbBad标记为true表示是一个坏地图点。用nvisible、nfound 暂存当前地图点的可视次数和被找到的次数并用变量mpReplaced指明是由pMP地图点替换掉的。

        遍历能观测到原地图点的pMPinKF所有关键帧：

        如果地图点pMP不在该帧观测中：添加观测

            // 该关键帧中没有对"要替换本地图点的地图点"的观测
            pKF->ReplaceMapPointMatch(mit->second, pMP);// 让KeyFrame用pMP替换掉原来的MapPoint
            pMP->AddObservation(pKF,mit->second);// 让MapPoint替换掉对应的KeyFrame

// 地图点的替换
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)
{
    mvpMapPoints[idx]=pMP;
}

/**
 * @brief 给地图点添加观测
 *
 * 记录哪些 KeyFrame 的那个特征点能观测到该 地图点
 * 并增加观测的相机数目nObs，单目+1，双目或者rgbd+2
 * 这个函数是建立关键帧共视关系的核心函数，能共同观测到某些地图点的关键帧是共视关键帧
 * @param pKF KeyFrame
 * @param idx MapPoint在KeyFrame中的索引
 */
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    // mObservations:观测到该MapPoint的关键帧KF和该MapPoint在KF中的索引
    // 如果已经添加过观测，返回
    if(mObservations.count(pKF)) 
        return;
    // 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
    mObservations[pKF]=idx;

    if(pKF->mvuRight[idx]>=0)
        nObs+=2; // 双目或者rgbd
    else
        nObs++; // 单目
}

        如果地图点pMP在该帧观测中：删除原地图点对观测

/**
 * @brief 由于其他的原因,导致当前关键帧观测到的某个地图点被删除(bad==true)了,将该地图点置为NULL
 * 
 * @param[in] idx   地图点在该关键帧中的id
 */
void KeyFrame::EraseMapPointMatch(const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    // NOTE 使用这种方式表示其中的某个地图点被删除
    mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}

        将原地图点的信息添加到新地图点上并更新最佳描述子

ORB-SLAM2 --- MapPoint::ComputeDistinctiveDescriptors 函数解析https://blog.csdn.net/qq_41694024/article/details/128515977