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 vpLocalCurrentWindowKFs;
    vector 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 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(), mSold_new.translation().cast());

        // 锁住altas更新地图
        unique_lock 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 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 currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
        unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map

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

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

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

        // 遍历每个融合帧所在地图的地图点
        for(MapPoint* pMPi : vpMergeMapMPs)
        {
            if(!pMPi || pMPi->isBad() || pMPi->GetMap() != pMergeMap)
                continue;

            // 把地图点添加到当前帧所在地图,从融合帧所在地图删掉
            pMPi->UpdateMap(pCurrentMap);
            pCurrentMap->AddMapPoint(pMPi);
            pMergeMap->EraseMapPoint(pMPi);
        }
        // ? BUG! pMergeMap没有设置为BAD
        // ? 应该加入如下代码吧？
        // ? mpAtlas->SetMapBad(pMergeMap);
        
        // Save non corrected poses (already merged maps)
        // 存下所有关键帧在融合矫正之前的位姿
        vector vpKFs = pCurrentMap->GetAllKeyFrames();
        for(KeyFrame* pKFi : vpKFs)
        {
            Sophus::SE3d Tiw = (pKFi->GetPose()).cast();
            g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0);
            NonCorrectedSim3[pKFi]=g2oSiw;
        }
    }

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

    //cout << "end updating current map" << endl;

    // Critical zone
    //bool good = pCurrentMap->CheckEssentialGraph();
    /*if(!good)
        cout << "BAD ESSENTIAL GRAPH!!" << endl;*/

    //cout << "Update essential graph" << endl;
    // mpCurrentKF->UpdateConnections(); // to put at false mbFirstConnection
    // Step 6 融合新旧地图的生成树
    pMergeMap->GetOriginKF()->SetFirstConnection(false);
    pNewChild = mpMergeMatchedKF->GetParent(); // Old parent, it will be the new child of this KF
    pNewParent = mpMergeMatchedKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent)
    mpMergeMatchedKF->ChangeParent(mpCurrentKF);
    while(pNewChild)
    {
        pNewChild->EraseChild(pNewParent); // We remove the relation between the old parent and the new for avoid loop
        KeyFrame * pOldParent = pNewChild->GetParent();
        pNewChild->ChangeParent(pNewParent);
        pNewParent = pNewChild;
        pNewChild = pOldParent;

    }


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

    //cout << "end update essential graph" << endl;

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 1!!" << endl;*/

    //cout << "Update relationship between KFs" << endl;
    vector vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge)
    vector vpCurrentConnectedKFs;

    // 为后续SearchAndFuse准备数据
    // 拿出融合帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧
    mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);
    vector aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
    mvpMergeConnectedKFs.insert(mvpMergeConnectedKFs.end(), aux.begin(), aux.end());
    if (mvpMergeConnectedKFs.size()>6)
        mvpMergeConnectedKFs.erase(mvpMergeConnectedKFs.begin()+6,mvpMergeConnectedKFs.end());
    /*mvpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
    mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);*/

    // 拿出当前关键帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧
    mpCurrentKF->UpdateConnections();
    vpCurrentConnectedKFs.push_back(mpCurrentKF);
    /*vpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    vpCurrentConnectedKFs.push_back(mpCurrentKF);*/
    aux = mpCurrentKF->GetVectorCovisibleKeyFrames();
    vpCurrentConnectedKFs.insert(vpCurrentConnectedKFs.end(), aux.begin(), aux.end());
    if (vpCurrentConnectedKFs.size()>6)
        vpCurrentConnectedKFs.erase(vpCurrentConnectedKFs.begin()+6,vpCurrentConnectedKFs.end());

    // 所有融合帧局部窗口的地图点
    set spMapPointMerge;
    for(KeyFrame* pKFi : mvpMergeConnectedKFs)
    {
        set vpMPs = pKFi->GetMapPoints();
        spMapPointMerge.insert(vpMPs.begin(),vpMPs.end());
        if(spMapPointMerge.size()>1000)
            break;
    }

    /*cout << "vpCurrentConnectedKFs.size() " << vpCurrentConnectedKFs.size() << endl;
    cout << "mvpMergeConnectedKFs.size() " << mvpMergeConnectedKFs.size() << endl;
    cout << "spMapPointMerge.size() " << spMapPointMerge.size() << endl;*/


    vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
    std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));
    //cout << "Finished to update relationship between KFs" << endl;

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

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 2!!" << endl;*/

    //cout << "start SearchAndFuse" << endl;
    // Step 7 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里，把重复的点融合掉（以旧换新）
    SearchAndFuse(vpCurrentConnectedKFs, vpCheckFuseMapPoint);
    //cout << "end SearchAndFuse" << endl;

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

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 3!!" << endl;

    cout << "Init to update connections" << endl;*/

    // 更新当前关键帧共视窗口内所有关键帧的连接
    for(KeyFrame* pKFi : vpCurrentConnectedKFs)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateConnections();
    }

    // 更新融合关键帧共视窗口内所有关键帧的连接
    for(KeyFrame* pKFi : mvpMergeConnectedKFs)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateConnections();
    }
    //cout << "end update connections" << endl;

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

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 4!!" << endl;*/

    // TODO Check: If new map is too small, we suppose that not informaiton can be propagated from new to old map
    if (numKFnew<10){
        mpLocalMapper->Release();
        return;
    }

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 5!!" << endl;*/

    // Perform BA
    bool bStopFlag=false;
    KeyFrame* pCurrKF = mpTracker->GetLastKeyFrame();
    //cout << "start MergeInertialBA" << endl;
    // Step 8 针对缝合区域的窗口内进行进行welding BA
    Optimizer::MergeInertialBA(pCurrKF, mpMergeMatchedKF, &bStopFlag, pCurrentMap,CorrectedSim3);
    //cout << "end MergeInertialBA" << endl;

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 6!!" << endl;*/

    // Release Local Mapping.
    mpLocalMapper->Release();


    return;
}

4.代码解析

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

    // Step 1 如果正在进行全局BA，停掉它
    if(isRunningGBA())
    {
        unique_lock 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();

        pCurrentMap保存着活跃地图（mpCurrentKF）的指针，pMergeMap保存着待融合地图（mpMergeMatchedKF）的指针。

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

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

        // 锁住altas更新地图
        unique_lock 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();
    }

        由于我们要融合地图，因此这里要用互斥锁锁住地图，利用mSold_new位姿（我们再在一致性校验的时候算出是当前关键帧世界坐标系到融合关键帧世界坐标系的变换）把整个当前地图中的关键帧和地图点变换到融合帧所在地图的坐标系下。

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

    // 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 lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
        // 用优化得到的 bias 更新普通帧位姿
        mpTracker->UpdateFrameIMU(1.0f,b,mpTracker->GetLastKeyFrame());

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

    }

        如果当前地图IMU未完成第三阶段初始化的话，我们帮助IMU快速完成初始化。

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

         这里和纯视觉不同，纯视觉是将地图点和关键帧融合到非活跃地图（非活跃地图吞并活跃地图变成活跃地图）里，这里是融合到活跃地图中（活跃地图一步步吞噬非活跃地图）。

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

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

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

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

        // 遍历每个融合帧所在地图的地图点
        for(MapPoint* pMPi : vpMergeMapMPs)
        {
            if(!pMPi || pMPi->isBad() || pMPi->GetMap() != pMergeMap)
                continue;

            // 把地图点添加到当前帧所在地图,从融合帧所在地图删掉
            pMPi->UpdateMap(pCurrentMap);
            pCurrentMap->AddMapPoint(pMPi);
            pMergeMap->EraseMapPoint(pMPi);
        }
        // ? BUG! pMergeMap没有设置为BAD
        // ? 应该加入如下代码吧？
        // ? mpAtlas->SetMapBad(pMergeMap);
        
        // Save non corrected poses (already merged maps)
        // 存下所有关键帧在融合矫正之前的位姿
        vector vpKFs = pCurrentMap->GetAllKeyFrames();
        for(KeyFrame* pKFi : vpKFs)
        {
            Sophus::SE3d Tiw = (pKFi->GetPose()).cast();
            g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0);
            NonCorrectedSim3[pKFi]=g2oSiw;
        }
    }

        这里和纯视觉相同，不做赘述，详细请参考我的博客：

ORBSLAM3 --- 地图融合（纯视觉模式下）LoopClosing::MergeLocal函数解析https://blog.csdn.net/qq_41694024/article/details/129637040        最后这里更新了NonCorrectedSim3变量，存储的是活跃地图的帧的未优化的在活跃地图坐标系下的坐标。

4.5 融合新旧地图的生成树

    pMergeMap->GetOriginKF()->SetFirstConnection(false);
    pNewChild = mpMergeMatchedKF->GetParent(); // Old parent, it will be the new child of this KF
    pNewParent = mpMergeMatchedKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent)
    mpMergeMatchedKF->ChangeParent(mpCurrentKF);
    while(pNewChild)
    {
        pNewChild->EraseChild(pNewParent); // We remove the relation between the old parent and the new for avoid loop
        KeyFrame * pOldParent = pNewChild->GetParent();
        pNewChild->ChangeParent(pNewParent);
        pNewParent = pNewChild;
        pNewChild = pOldParent;

    }

        这里和纯视觉相同，不做赘述，详细请参考我的博客：

ORBSLAM3 --- 地图融合（纯视觉模式下）LoopClosing::MergeLocal函数解析https://blog.csdn.net/qq_41694024/article/details/129637040

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

    //cout << "Update relationship between KFs" << endl;
    vector vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge)
    vector vpCurrentConnectedKFs;

    // 为后续SearchAndFuse准备数据
    // 拿出融合帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧
    mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);
    vector aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
    mvpMergeConnectedKFs.insert(mvpMergeConnectedKFs.end(), aux.begin(), aux.end());
    if (mvpMergeConnectedKFs.size()>6)
        mvpMergeConnectedKFs.erase(mvpMergeConnectedKFs.begin()+6,mvpMergeConnectedKFs.end());
    /*mvpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
    mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);*/

    // 拿出当前关键帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧
    mpCurrentKF->UpdateConnections();
    vpCurrentConnectedKFs.push_back(mpCurrentKF);
    /*vpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    vpCurrentConnectedKFs.push_back(mpCurrentKF);*/
    aux = mpCurrentKF->GetVectorCovisibleKeyFrames();
    vpCurrentConnectedKFs.insert(vpCurrentConnectedKFs.end(), aux.begin(), aux.end());
    if (vpCurrentConnectedKFs.size()>6)
        vpCurrentConnectedKFs.erase(vpCurrentConnectedKFs.begin()+6,vpCurrentConnectedKFs.end());

    // 所有融合帧局部窗口的地图点
    set spMapPointMerge;
    for(KeyFrame* pKFi : mvpMergeConnectedKFs)
    {
        set vpMPs = pKFi->GetMapPoints();
        spMapPointMerge.insert(vpMPs.begin(),vpMPs.end());
        if(spMapPointMerge.size()>1000)
            break;
    }

    /*cout << "vpCurrentConnectedKFs.size() " << vpCurrentConnectedKFs.size() << endl;
    cout << "mvpMergeConnectedKFs.size() " << mvpMergeConnectedKFs.size() << endl;
    cout << "spMapPointMerge.size() " << spMapPointMerge.size() << endl;*/


    vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
    std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));
    //cout << "Finished to update relationship between KFs" << endl;

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

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 2!!" << endl;*/

    //cout << "start SearchAndFuse" << endl;
    // Step 7 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里，把重复的点融合掉（以旧换新）
    SearchAndFuse(vpCurrentConnectedKFs, vpCheckFuseMapPoint);

        这段我们未融合地图点进行准备：

        mvpMergeConnectedKFs存储着融合关键帧和其五个共视关键帧。

        vpCurrentConnectedKFs存储着当前关键帧和其五个共视关键帧。

        spMapPointMerge存储着融合关键帧的1000个地图点。

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

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

    // 更新当前关键帧共视窗口内所有关键帧的连接
    for(KeyFrame* pKFi : vpCurrentConnectedKFs)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateConnections();
    }

    // 更新融合关键帧共视窗口内所有关键帧的连接
    for(KeyFrame* pKFi : mvpMergeConnectedKFs)
    {
        if(!pKFi || pKFi->isBad())
            continue;

        pKFi->UpdateConnections();
    }
    //cout << "end update connections" << endl;

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

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 4!!" << endl;*/

    // TODO Check: If new map is too small, we suppose that not informaiton can be propagated from new to old map
    if (numKFnew<10){
        mpLocalMapper->Release();
        return;
    }

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 5!!" << endl;*/

    // Perform BA
    bool bStopFlag=false;
    KeyFrame* pCurrKF = mpTracker->GetLastKeyFrame();
    //cout << "start MergeInertialBA" << endl;
    // Step 8 针对缝合区域的窗口内进行进行welding BA
    Optimizer::MergeInertialBA(pCurrKF, mpMergeMatchedKF, &bStopFlag, pCurrentMap,CorrectedSim3);
    //cout << "end MergeInertialBA" << endl;

    /*good = pCurrentMap->CheckEssentialGraph();
    if(!good)
        cout << "BAD ESSENTIAL GRAPH 6!!" << endl;*/

    // Release Local Mapping.
    mpLocalMapper->Release();

        numKFnew保存着当前地图的关键帧的数量，如果小于10开启局部建图线程并不进行BA。（为什么我不是很理解，可能关键帧太少融合weilding BA效果不太好吧？？？）