ORBSLAM3 --- 地图融合（惯性模式下）LoopClosing::MergeLocal2函数解析

目录

1.函数作用

2.函数流程

3.代码详细注释

4.代码解析

4.1 中止全局BA，结束局部建图线程，获得地图指针

4.2 利用前面计算的坐标系变换位姿，把整个当前地图（关键帧及地图点）变换到融合帧所在地图

4.3 如果当前地图IMU没有完全初始化，帮助IMU快速优化

4.4 地图以旧换新，把融合帧所在地图里的关键帧和地图点从原地图里删掉，变更为当前关键帧所在地图。

4.5 融合新旧地图的生成树

4.6 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里，把重复的点融合掉（以旧换新）

4.7 更新当前关键帧共视窗口内所有关键帧的连接，针对缝合区域的窗口内进行进行welding BA

---

1.函数作用

        在IMU的模式下进行地图融合。

2.函数流程

        特点:
        只对缝合区域进行了welding BA。其他地图区域是直接位姿变换过来了，没有类似纯视觉地图融合里的本质图优化或全局优化。
        如果当前地图IMU没有完全初始化，那么再进行一次IMU快速优化后，强制认为已经完成IMU初始化。
        地图以旧换新。在纯视觉地图里是以新换旧。

        步骤:
        1. 停掉正在进行的全局BA、局部建图线程。
        2. 利用前面计算的坐标系变换位姿gSw2w1,把整个当前地图(关键帧及地图点)变换到融合帧所在地图。
        3. 如果当前地图IMU没有完全初始化,帮助IMU快速优化
        4. 地图以旧换新。把融合帧所在地图里的关键帧和地图点从原地图里删掉,变更为当前关键帧所在地图。
        5. 融合新旧地图的生成树
        6. 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里,把重复的点融合掉(以旧换新)

        7. 针对缝合区域的窗口内进行welding BA。共视关键帧只优化位姿，不优化IMU参数。

3.代码详细注释

/**
 * @brief 惯性模式下的地图融合
 */
void LoopClosing::MergeLocal2()
{
    //cout << "Merge detected!!!!" << endl;
    // 没用上
    int numTemporalKFs = 11; //TODO (set by parameter): Temporal KFs in the local window if the map is inertial.

    //Relationship to rebuild the essential graph, it is used two times, first in the local window and later in the rest of the map
    // 用来重新构造Essential Graph
    KeyFrame* pNewChild;
    KeyFrame* pNewParent;

    // 没用上
    vector<KeyFrame*> vpLocalCurrentWindowKFs;
    vector<KeyFrame*> vpMergeConnectedKFs;

    // 记录用初始Sim3 计算出来的当前关键帧局部共视帧窗口内的所有关键帧矫正前的值和矫正后的初始值
    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
    // NonCorrectedSim3[mpCurrentKF]=mg2oLoopScw;

    // Flag that is true only when we stopped a running BA, in this case we need relaunch at the end of the merge
    // 记录要不要重新进行全局ba
    bool bRelaunchBA = false;

    //cout << "Check Full Bundle Adjustment" << endl;
    // If a Global Bundle Adjustment is running, abort it
    // Step 1 如果正在进行全局BA，停掉它
    if(isRunningGBA())
    {
        unique_lock<mutex> lock(mMutexGBA);
        mbStopGBA = true;

        mnFullBAIdx++;

        if(mpThreadGBA)
        {
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
        bRelaunchBA = true;
    }


    //cout << "Request Stop Local Mapping" << endl;
    // Step 2 暂停局部建图线程
    mpLocalMapper->RequestStop();
    // Wait until Local Mapping has effectively stopped
    // 等待直到完全停掉
    while(!mpLocalMapper->isStopped())
    {
        usleep(1000);
    }
    //cout << "Local Map stopped" << endl;

    // 当前关键帧地图的指针
    Map* pCurrentMap = mpCurrentKF->GetMap();
    // 融合关键帧地图的指针
    Map* pMergeMap = mpMergeMatchedKF->GetMap();

    // Step 3 利用前面计算的坐标系变换位姿，把整个当前地图（关键帧及地图点）变换到融合帧所在地图
    {
        // 把当前关键帧所在的地图位姿带到融合关键帧所在的地图
        // mSold_new = gSw2w1 记录的是当前关键帧世界坐标系到融合关键帧世界坐标系的变换
        float s_on = mSold_new.scale();
        Sophus::SE3f T_on(mSold_new.rotation().cast<float>(), mSold_new.translation().cast<float>());

        // 锁住altas更新地图
        unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);

        //cout << "KFs before empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl;
        // 队列里还没来得及处理的关键帧清空
        mpLocalMapper->EmptyQueue();
        //cout << "KFs after empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl;

        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
        //cout << "updating active map to merge reference" << endl;
        //cout << "curr merge KF id: " << mpCurrentKF->mnId << endl;
        //cout << "curr tracking KF id: " << mpTracker->GetLastKeyFrame()->mnId << endl;
        // 是否将尺度更新到速度
        bool bScaleVel=false;
        if(s_on!=1)  // ?判断浮点数和1严格相等是不是不合适？
            bScaleVel=true;
        // 利用mSold_new位姿把整个当前地图中的关键帧和地图点变换到融合帧所在地图的坐标系下
        mpAtlas->GetCurrentMap()->ApplyScaledRotation(T_on,s_on,bScaleVel);
        // 尺度更新到普通帧位姿
        mpTracker->UpdateFrameIMU(s_on,mpCurrentKF->GetImuBias(),mpTracker->GetLastKeyFrame());

        std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
    }

    // Step 4 如果当前地图IMU没有完全初始化，帮助IMU快速优化；
    // 反正都要融合了，这里就拔苗助长完成IMU优化，回头直接全部放到融合地图里就好了
    // 如果没有完成IMU的第三阶段初始化
    const int numKFnew=pCurrentMap->KeyFramesInMap();

    if((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD)
       && !pCurrentMap->GetIniertialBA2())
    {
        // Map is not completly initialized
        Eigen::Vector3d bg, ba;
        bg << 0., 0., 0.;
        ba << 0., 0., 0.;
        // 优化当前地图中参数bg,ba
        Optimizer::InertialOptimization(pCurrentMap,bg,ba);
        IMU::Bias b (ba[0],ba[1],ba[2],bg[0],bg[1],bg[2]);
        unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
        // 用优化得到的 bias 更新普通帧位姿
        mpTracker->UpdateFrameIMU(1.0f,b,mpTracker->GetLastKeyFrame());

        // Set map initialized
        // 设置IMU已经完成初始化
        pCurrentMap->SetIniertialBA2();
        pCurrentMap->SetIniertialBA1();
        pCurrentMap->SetImuInitialized();

    }


    //cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << "       CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl;

    // Load KFs and MPs from merge map
    //cout << "updating current map" << endl;
    // Step 5 地图以旧换新。把融合帧所在地图里的关键帧和地图点从原地图里删掉，变更为当前关键帧所在地图。
    {
        // Get Merge Map Mutex (This section stops tracking!!)
        // 锁住两张地图
        unique_lock<mutex> currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
        unique_lock<mutex> mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map

        // 融合帧所在地图的所有关键帧和地图点
        vector<KeyFrame*> vpMergeMapKFs = pMergeMap->GetAllKeyFrames();
        vector<MapPoint*> vpMergeMapMPs = pMergeMap->GetAllMapPoints();

        // 遍历每个融合帧所在地图的关键帧
        for(KeyFrame* pKFi : vpMergeMapKFs)
        {
            if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pMergeMap)
            {
                continue;
            }

            // Make sure connections are updated
            // 把该关键帧从融合帧所在地图删掉,加入到当前的地图中
            pKFi->UpdateMap(pCurrentMap);