• ORB-SLAM2从理论到代码实现（十）：LoopClosing程序详解

    LoopClosing是专门负责做闭环的类，它的主要功能就是检测闭环，计算闭环帧的相对位姿病以此做闭环修正。

    1. 成员变量

    1. std::list mlpLoopKeyFrameQueue; // 关键帧队列
    2.  
    3. std::mutex mMutexLoopQueue;
    4.  
    5. // Loop detector parameters
    6. float mnCovisibilityConsistencyTh;
    7.  
    8. // Loop detector variables
    9. KeyFrame* mpCurrentKF;
    10. KeyFrame* mpMatchedKF;
    11. std::vector mvConsistentGroups;
    12. std::vector mvpEnoughConsistentCandidates;
    13. std::vector mvpCurrentConnectedKFs;
    14. std::vector mvpCurrentMatchedPoints;
    15. std::vector mvpLoopMapPoints;
    16. cv::Mat mScw;
    17. g2o::Sim3 mg2oScw;

    2. Run主循环

    1. void LoopClosing::Run()
    2. {
    3.     mbFinished =false;
    4.  
    5.     while(1)
    6.     {
    7.         // Check if there are keyframes in the queue
    8.         if(CheckNewKeyFrames())
    9.         {
    10.             // Detect loop candidates and check covisibility consistency
    11.             if(DetectLoop())
    12.             {
    13.                // Compute similarity transformation [sR|t]
    14.                // In the stereo/RGBD case s=1
    15.                // 单目情况因为尺度是不可观的，所以是sim3，双目、RGBD、有IMU等有尺度的情况下，变为SE3
    16.                if(ComputeSim3()) // 主要是判断该闭环是否可靠，只有在十分可靠的情况下才进行闭环计算
    17.                {
    18.                    // Perform loop fusion and pose graph optimization
    19.                    CorrectLoop();
    20.                }
    21.             }
    22.         }       
    23.  
    24.         ResetIfRequested();
    25.  
    26.         if(CheckFinish())
    27.             break;
    28.  
    29.         usleep(5000);
    30.     }
    31.  
    32.     SetFinish();
    33. }

    2. InsertKeyFrame插入关键帧

    LocalMapping模块处理完关键帧之后把它插入进来

    1. void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
    2. {
    3.     unique_lock lock(mMutexLoopQueue);
    4.     if(pKF->mnId!=0)
    5.         mlpLoopKeyFrameQueue.push_back(pKF);
    6. }

    3. DetectLoop：检测闭环

    它的主要流程包括：

    1）如果地图中的关键帧数小于10，那么不进行闭环检测

    2）获取共视关键帧，并计算他们和当前关键帧之间的BoW分数，求得最低分

    3）通过上一步计算出的最低分数到数据库中查找出候选关键帧，这一步相当于是找到了曾经到过此处的关键帧们

    4）对候选关键帧集进行连续性检测

    1. bool LoopClosing::DetectLoop()
    2. {
    3.   {
    4.     // 取出队首的关键帧，也就是最旧的关键帧
    5.     unique_lock lock(mMutexLoopQueue);
    6.     mpCurrentKF = mlpLoopKeyFrameQueue.front();
    7.     mlpLoopKeyFrameQueue.pop_front();
    8.     // Avoid that a keyframe can be erased while it is being process by this thread
    9.     mpCurrentKF->SetNotErase();
    10.   }
    11.  
    12.   //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
    13.   // 如果距离上次闭环小于10帧，则不进行闭环检测
    14.   if(mpCurrentKF->mnId10)
    15.   {
    16.     mpKeyFrameDB->add(mpCurrentKF);
    17.     mpCurrentKF->SetErase();
    18.     return false;
    19.   }
    20.  
    21.   // Compute reference BoW similarity score
    22.   // This is the lowest score to a connected keyframe in the covisibility graph
    23.   // We will impose loop candidates to have a higher similarity than this
    24.   //计算当前帧及其共视关键帧的词袋模型匹配得分，获得最小得分minScore
    25.   const vector vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    26.   const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    27.   float minScore = 1;
    28.   for(size_t i=0; isize(); i++)
    29.   {
    30.     KeyFrame* pKF = vpConnectedKeyFrames[i];
    31.     if(pKF->isBad())
    32.       continue;
    33.     const DBoW2::BowVector &BowVec = pKF->mBowVec;
    34.  
    35.     float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
    36.  
    37.     if(score
    38.       minScore = score;
    39.   }
    40.  
    41.   // Query the database imposing the minimum score
    42.   //在除去当前帧共视关系的关键帧数据中，检测闭环候选帧(这个函数在KeyFrameDatabase中)
    43.   //闭环候选帧删选过程：
    44.   //1,BoW得分>minScore;
    45.   //2,统计满足1的关键帧中有共同单子最多的单词数maxcommonwords
    46.   //3,筛选出共同单词数大于mincommons(=0.8*maxcommons)的关键帧
    47.   //4,相连的关键帧分为一组，计算组得分（总分）,得到最大总分bestAccScore,筛选出总分大于minScoreToRetain(=0.75*bestAccScore)的组
    48.   //用得分最高的候选帧IAccScoreAndMathch代表该组，计算组得分的目的是剔除单独一帧得分较高，但是没有共视关键帧作为闭环来说不够鲁棒
    49.   //对于通过了闭环检测的关键帧，还需要通过连续性检测(连续三帧都通过上面的筛选)，才能作为闭环候选帧
    50.   vector vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
    51.  
    52.   // If there are no loop candidates, just add new keyframe and return false
    53.   if(vpCandidateKFs.empty())
    54.   {
    55.     mpKeyFrameDB->add(mpCurrentKF);
    56.     mvConsistentGroups.clear();
    57.     mpCurrentKF->SetErase();
    58.     return false;
    59.   }
    60.  
    61.   // For each loop candidate check consistency with previous loop candidates
    62.   // Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)
    63.   // A group is consistent with a previous group if they share at least a keyframe
    64.   // We must detect a consistent loop in several consecutive keyframes to accept it
    65.   // 在候选帧中检测具有连续性的候选帧
    66.   // 每个候选帧将与自己相连的关键帧构成一个“候选组spCandidateGroup”
    67.   // 检测“候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，将该“候选组”放入“当前连续组vCurrentConsistentGroups”
    68.   // 如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
    69.   mvpEnoughConsistentCandidates.clear();
    70.  
    71.   vector vCurrentConsistentGroups;
    72.   vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
    73.   for(size_t i=0, iend=vpCandidateKFs.size(); i
    74.   {
    75.     KeyFrame* pCandidateKF = vpCandidateKFs[i];
    76.  
    77.     // 将自己以及与自己相连的关键帧构成一个“候选组”
    78.     set spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
    79.     spCandidateGroup.insert(pCandidateKF);
    80.  
    81.     bool bEnoughConsistent = false;
    82.     bool bConsistentForSomeGroup = false;
    83.     // 遍历之前的“连续组”
    84.     for(size_t iG=0, iendG=mvConsistentGroups.size(); iG
    85.     {
    86.       // 取出一个之前的连续组
    87.       set sPreviousGroup = mvConsistentGroups[iG].first;
    88.  
    89.       // 遍历每个“候选组”，检测候选组中每一个关键帧在“连续组”中是否存在
    90.       bool bConsistent = false;
    91.       for(set::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
    92.       {
    93.         if(sPreviousGroup.count(*sit))
    94.         {
    95.           bConsistent=true;
    96.           bConsistentForSomeGroup=true;// 该“候选组”至少与一个”连续组“相连
    97.           break;
    98.         }
    99.       }
    100.  
    101.       if(bConsistent)
    102.       {
    103.         int nPreviousConsistency = mvConsistentGroups[iG].second;
    104.         int nCurrentConsistency = nPreviousConsistency + 1;
    105.         if(!vbConsistentGroup[iG])
    106.         {
    107.           ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
    108.           vCurrentConsistentGroups.push_back(cg);
    109.           vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
    110.         }
    111.         if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
    112.         {
    113.           mvpEnoughConsistentCandidates.push_back(pCandidateKF);
    114.           bEnoughConsistent=true; //this avoid to insert the same candidate more than once
    115.         }
    116.       }
    117.     }
    118.  
    119.     // If the group is not consistent with any previous group insert with consistency counter set to zero
    120.     // 如果该“候选组”的所有关键帧都不存在于“连续组”，那么vCurrentConsistentGroups将为空，
    121.     // 于是就把“子候选组”全部拷贝到vCurrentConsistentGroups，并最终用于更新mvConsistentGroups，计数设为0，重新开始
    122.     if(!bConsistentForSomeGroup)
    123.     {
    124.       ConsistentGroup cg = make_pair(spCandidateGroup,0);
    125.       vCurrentConsistentGroups.push_back(cg);
    126.     }
    127.   }
    128.  
    129.   // Update Covisibility Consistent Groups
    130.   mvConsistentGroups = vCurrentConsistentGroups;
    131.  
    132.  
    133.   // Add Current Keyframe to database
    134.   mpKeyFrameDB->add(mpCurrentKF);
    135.  
    136.   if(mvpEnoughConsistentCandidates.empty())
    137.   {
    138.     mpCurrentKF->SetErase();
    139.     return false;
    140.   }
    141.   else
    142.   {
    143.     return true;
    144.   }
    145.  
    146.   mpCurrentKF->SetErase();
    147.   return false;
    148. }

    2. ComputeSim3：计算两帧之间的相对位姿

    主要流程包括：

    1）对每一个闭环帧，通过BoW的matcher方法进行第一次匹配，匹配闭环帧和当前关键帧之间的匹配关系，如果对应关系少于20个，则丢弃，否则构造一个Sim3求解器并保存起来。

    2）对上一步得到的每一个满足条件的闭环帧，通过RANSAC迭代，求解Sim3。

    3）通过返回的Sim3进行第二次匹配。

    4）使用非线性最小二乘法优化Sim3.

    5）使用非线性最小二乘法优化Sim3.

    6）使用投影得到更多的匹配点，如果匹配点数量充足，则接受该闭环。

    1. bool LoopClosing::ComputeSim3()
    2. {
    3.     // For each consistent loop candidate we try to compute a Sim3
    4.  
    5.     const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
    6.  
    7.     // We compute first ORB matches for each candidate
    8.     // If enough matches are found, we setup a Sim3Solver
    9.     ORBmatcher matcher(0.75,true);
    10.  
    11.     vector vpSim3Solvers;
    12.     vpSim3Solvers.resize(nInitialCandidates);
    13.  
    14.     vector > vvpMapPointMatches;
    15.     vvpMapPointMatches.resize(nInitialCandidates);
    16.  
    17.     vector<bool> vbDiscarded;
    18.     vbDiscarded.resize(nInitialCandidates);
    19.  
    20.     int nCandidates=0; //candidates with enough matches
    21.  
    22.     for(int i=0; i
    23.     {
    24.         // 闭环候选帧中取出一帧关键帧pKF
    25.         KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
    26.  
    27.         // avoid that local mapping erase it while it is being processed in this thread
    28.         // 防止在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
    29.         pKF->SetNotErase();
    30.  
    31.         if(pKF->isBad())
    32.         {
    33.             vbDiscarded[i] = true;
    34.             continue;
    35.         }
    36.  
    37.         // 将当前帧mpCurrentKF与闭环候选关键帧pKF匹配
    38.         // 通过bow加速得到mpCurrentKF与pKF之间的匹配特征点，vvpMapPointMatches是匹配特征点对应的MapPoints
    39.         int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
    40.  
    41.         // 匹配的特征点数太少，剔除
    42.         if(nmatches<20)
    43.         {
    44.             vbDiscarded[i] = true;
    45.             continue;
    46.         }
    47.         else
    48.         {
    49.             // 构造Sim3求解器
    50.             // 如果mbFixScale为true，则是6DoFf优化，如果是false，则是7DoF优化（单目）
    51.             Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
    52.             pSolver->SetRansacParameters(0.99,20,300);
    53.             vpSim3Solvers[i] = pSolver;
    54.         }
    55.         // 参与Sim3计算的候选关键帧数加1
    56.         nCandidates++;
    57.     }
    58.  
    59.     bool bMatch = false;// 标记是否有一个候选帧通过Sim3的求解
    60.  
    61.     // Perform alternatively RANSAC iterations for each candidate
    62.     // until one is succesful or all fail
    63.     // 一直循环所有的候选帧，每个候选帧迭代5次，如果5次迭代后得不到结果，就换下一个候选帧
    64.     // 直到有一个候选帧首次迭代成功，或者某个候选帧总的迭代次数超过限制，直接将它剔除
    65.     while(nCandidates>0 && !bMatch)
    66.     {
    67.         for(int i=0; i
    68.         {
    69.             if(vbDiscarded[i])
    70.                 continue;
    71.  
    72.             KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
    73.  
    74.             // Perform 5 Ransac Iterations
    75.             vector<bool> vbInliers;
    76.             int nInliers;
    77.             bool bNoMore;
    78.  
    79.             // 对有较好的匹配的关键帧求取Sim3变换
    80.             Sim3Solver* pSolver = vpSim3Solvers[i];
    81.             cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
    82.  
    83.             // If Ransac reachs max. iterations discard keyframe
    84.             // 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
    85.             if(bNoMore)
    86.             {
    87.                 vbDiscarded[i]=true;
    88.                 nCandidates--;
    89.             }
    90.  
    91.             // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
    92.             if(!Scm.empty())
    93.             {
    94.                 vector vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast(NULL));
    95.                 for(size_t j=0, jend=vbInliers.size(); j
    96.                 {
    97.                     // 保存inlier的MapPoint
    98.                     if(vbInliers[j])
    99.                        vpMapPointMatches[j]=vvpMapPointMatches[i][j];
    100.                 }
    101.                 // 通过步骤3求取的Sim3变换引导关键帧匹配弥补步骤2中的漏匹配
    102.                 cv::Mat R = pSolver->GetEstimatedRotation();
    103.                 cv::Mat t = pSolver->GetEstimatedTranslation();
    104.                 const float s = pSolver->GetEstimatedScale();
    105.                 // 查找更多的匹配 使用SearchByBoW进行特征点匹配时会有漏匹配
    106.                 // 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
    107.                 // 在该区域内通过描述子进行匹配pKF1和pKF2之前漏匹配的特征点，更新匹配vpMapPointMatches
    108.                 matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
    109.  
    110.                 // Sim3优化，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
    111.                 g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
    112.                 // 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
    113.                 const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
    114.  
    115.                 // If optimization is succesful stop ransacs and continue
    116.                 if(nInliers>=20)
    117.                 {
    118.                     bMatch = true;
    119.                     // mpMatchedKF是最终闭环检测出来与当前帧形成闭环的关键帧
    120.                     mpMatchedKF = pKF;
    121.                     // 得到从世界坐标系到该候选帧的Sim3变换
    122.                     g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
    123.                     // 得到优化后从世界坐标系到当前帧的Sim3变换
    124.                     mg2oScw = gScm*gSmw;
    125.                     mScw = Converter::toCvMat(mg2oScw);
    126.  
    127.                     mvpCurrentMatchedPoints = vpMapPointMatches;
    128.                     break;//跳出对其它候选帧的判断
    129.                 }
    130.             }
    131.         }
    132.     }
    133.  
    134.     // 没有一个闭环匹配候选帧通过Sim3的求解与优化
    135.     if(!bMatch)
    136.     {
    137.         for(int i=0; i
    138.              mvpEnoughConsistentCandidates[i]->SetErase();
    139.         mpCurrentKF->SetErase();
    140.         return false;
    141.     }
    142.  
    143.     // Retrieve MapPoints seen in Loop Keyframe and neighbors
    144.     // 取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入mvpLoopMapPoints
    145.     // 将mpMatchedKF相连的关键帧全部取出来放入vpLoopConnectedKFs
    146.     vector vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
    147.     vpLoopConnectedKFs.push_back(mpMatchedKF);
    148.     mvpLoopMapPoints.clear();
    149.     for(vector::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
    150.     {
    151.         KeyFrame* pKF = *vit;
    152.         vector vpMapPoints = pKF->GetMapPointMatches();
    153.         for(size_t i=0, iend=vpMapPoints.size(); i
    154.         {
    155.             MapPoint* pMP = vpMapPoints[i];
    156.             if(pMP)
    157.             {
    158.                 if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
    159.                 {
    160.                     mvpLoopMapPoints.push_back(pMP);
    161.                     pMP->mnLoopPointForKF=mpCurrentKF->mnId;
    162.                 }
    163.             }
    164.         }
    165.     }
    166.  
    167.     // Find more matches projecting with the computed Sim3
    168.     // 将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
    169.     // 根据投影查找更多的匹配
    170.     // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域，
    171.     // 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新mvpCurrentMatchedPoints
    172.     matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
    173.  
    174.     // If enough matches accept Loop
    175.     // 断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
    176.     int nTotalMatches = 0;
    177.     for(size_t i=0; isize(); i++)
    178.     {
    179.         if(mvpCurrentMatchedPoints[i])
    180.             nTotalMatches++;
    181.     }
    182.  
    183.     if(nTotalMatches>=40)
    184.     {
    185.         for(int i=0; i
    186.             if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
    187.                 mvpEnoughConsistentCandidates[i]->SetErase();
    188.         return true;
    189.     }
    190.     else
    191.     {
    192.         for(int i=0; i
    193.             mvpEnoughConsistentCandidates[i]->SetErase();
    194.         mpCurrentKF->SetErase();
    195.         return false;
    196.     }
    197.  
    198. }

    3. CorrectLoop：根据闭环做校正

    主要流程包括：

    1）如果有全局BA运算在运行的话，终止之前的BA运算。

    2）使用传播法计算每一个关键帧正确的Sim3变换值

    3）优化图

    4）全局BA优化

    1. void LoopClosing::CorrectLoop()
    2. {
    3.     cout << "Loop detected!" << endl;
    4.  
    5.     // Send a stop signal to Local Mapping
    6.     // Avoid new keyframes are inserted while correcting the loop
    7.     //请求局部地图停止
    8.     mpLocalMapper->RequestStop();
    9.  
    10.     // If a Global Bundle Adjustment is running, abort it
    11.     if(isRunningGBA())
    12.     {
    13.         unique_lock lock(mMutexGBA);
    14.         mbStopGBA = true;
    15.  
    16.         mnFullBAIdx++;
    17.  
    18.         if(mpThreadGBA)
    19.         {
    20.             mpThreadGBA->detach();
    21.             delete mpThreadGBA;
    22.         }
    23.     }
    24.  
    25.     // Wait until Local Mapping has effectively stopped
    26.     while(!mpLocalMapper->isStopped())
    27.     {
    28.         usleep(1000);
    29.     }
    30.  
    31.     // Ensure current keyframe is updated
    32.     // 根据共视关系更新当前帧与其它关键帧之间的连接
    33.     mpCurrentKF->UpdateConnections();
    34.  
    35.     // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
    36.     // 得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
    37.     // 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换
    38.     // 取出与当前帧相连的关键帧，包括当前关键帧
    39.     mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    40.     mvpCurrentConnectedKFs.push_back(mpCurrentKF);
    41.  
    42.     KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
    43.     CorrectedSim3[mpCurrentKF]=mg2oScw;
    44.     cv::Mat Twc = mpCurrentKF->GetPoseInverse();
    45.  
    46.  
    47.     {
    48.         // Get Map Mutex
    49.         unique_lock lock(mpMap->mMutexMapUpdate);
    50.  
    51.         // 得到Sim3调整后其它与当前帧相连关键帧的位姿
    52.         for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
    53.         {
    54.             KeyFrame* pKFi = *vit;
    55.  
    56.             cv::Mat Tiw = pKFi->GetPose();
    57.  
    58.             if(pKFi!=mpCurrentKF)
    59.             {
    60.                 // 得到当前帧到pKFi帧的相对变换
    61.                 cv::Mat Tic = Tiw*Twc;
    62.                 cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
    63.                 cv::Mat tic = Tic.rowRange(0,3).col(3);
    64.                 g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
    65.                 // 当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿
    66.                 g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
    67.                 //Pose corrected with the Sim3 of the loop closure
    68.                 // 得到闭环g2o优化后各个关键帧的位姿
    69.                 CorrectedSim3[pKFi]=g2oCorrectedSiw;
    70.             }
    71.  
    72.             cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
    73.             cv::Mat tiw = Tiw.rowRange(0,3).col(3);
    74.             g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
    75.             //Pose without correction
    76.             // 当前帧相连关键帧，没有进行闭环优化的位姿
    77.             NonCorrectedSim3[pKFi]=g2oSiw;
    78.         }
    79.  
    80.         // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
    81.         // 上一步得到调整相连帧位姿后，修正这些关键帧的地图点
    82.         for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
    83.         {
    84.             KeyFrame* pKFi = mit->first;
    85.             g2o::Sim3 g2oCorrectedSiw = mit->second;
    86.             g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
    87.  
    88.             g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];
    89.  
    90.             vector vpMPsi = pKFi->GetMapPointMatches();
    91.             for(size_t iMP=0, endMPi = vpMPsi.size(); iMP
    92.             {
    93.                 MapPoint* pMPi = vpMPsi[iMP];
    94.                 if(!pMPi)
    95.                     continue;
    96.                 if(pMPi->isBad())
    97.                     continue;
    98.                 if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId)
    99.                     continue;
    100.  
    101.                 // Project with non-corrected pose and project back with corrected pose
    102.                 cv::Mat P3Dw = pMPi->GetWorldPos();
    103.                 Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
    104.                 Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
    105.  
    106.                 cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
    107.                 pMPi->SetWorldPos(cvCorrectedP3Dw);
    108.                 pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
    109.                 pMPi->mnCorrectedReference = pKFi->mnId;
    110.                 pMPi->UpdateNormalAndDepth();
    111.             }
    112.  
    113.             // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
    114.             Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
    115.             Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
    116.             double s = g2oCorrectedSiw.scale();
    117.  
    118.             eigt *=(1./s); //[R t/s;0 1]
    119.  
    120.             cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
    121.  
    122.             pKFi->SetPose(correctedTiw);
    123.  
    124.             // Make sure connections are updated
    125.             pKFi->UpdateConnections();
    126.         }
    127.  
    128.         // Start Loop Fusion
    129.         // Update matched map points and replace if duplicated
    130.         // 检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
    131.         for(size_t i=0; isize(); i++)
    132.         {
    133.             if(mvpCurrentMatchedPoints[i])
    134.             {
    135.                 MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
    136.                 MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
    137.                 if(pCurMP) // 如果有重复的MapPoint，则用匹配帧的代替现有的
    138.                     pCurMP->Replace(pLoopMP);
    139.                 else // 如果当前帧没有该MapPoint，则直接添加
    140.                 {
    141.                     mpCurrentKF->AddMapPoint(pLoopMP,i);
    142.                     pLoopMP->AddObservation(mpCurrentKF,i);
    143.                     pLoopMP->ComputeDistinctiveDescriptors();
    144.                 }
    145.             }
    146.         }
    147.  
    148.     }
    149.  
    150.     // Project MapPoints observed in the neighborhood of the loop keyframe
    151.     // into the current keyframe and neighbors using corrected poses.
    152.     // Fuse duplications.
    153.     // 通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
    154.     SearchAndFuse(CorrectedSim3);
    155.  
    156.  
    157.     // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
    158.     // 更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
    159.     map > LoopConnections;
    160.  
    161.     // 遍历当前帧相连关键帧
    162.     for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
    163.     {
    164.         KeyFrame* pKFi = *vit;
    165.         // 得到与当前帧相连关键帧的相连关键帧（二级相连）
    166.         vector vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
    167.  
    168.         // Update connections. Detect new links.
    169.         // 更新一级相连关键帧的连接关系
    170.         pKFi->UpdateConnections();
    171.         // 取出该帧更新后的连接关系
    172.         LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
    173.         // 从连接关系中去除闭环之前的二级连接关系，剩下的是由闭环得到的连接关系
    174.         for(vector::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
    175.         {
    176.             LoopConnections[pKFi].erase(*vit_prev);
    177.         }
    178.         // 从连接关系中去除闭环之前的一级连接关系，剩下的是由闭环得到的连接关系
    179.         for(vector::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
    180.         {
    181.             LoopConnections[pKFi].erase(*vit2);
    182.         }
    183.     }
    184.  
    185.     // Optimize graph
    186.     // 进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系
    187.     Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
    188.  
    189.     mpMap->InformNewBigChange();
    190.  
    191.     // Add loop edge
    192.     // 添加当前帧与闭环匹配帧之间的边
    193.     mpMatchedKF->AddLoopEdge(mpCurrentKF);
    194.     mpCurrentKF->AddLoopEdge(mpMatchedKF);
    195.  
    196.     // Launch a new thread to perform Global Bundle Adjustment
    197.     // 新建一个线程用于全局BA优化
    198.     mbRunningGBA = true;
    199.     mbFinishedGBA = false;
    200.     mbStopGBA = false;
    201.     mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
    202.  
    203.     // Loop closed. Release Local Mapping.
    204.     mpLocalMapper->Release();    
    205.  
    206.     mLastLoopKFid = mpCurrentKF->mnId;   
    207. }

    4. MergeLocal

    1. void LoopClosing::MergeLocal()
    2. {
    3.     Verbose::PrintMess("MERGE: Merge Visual detected!!!!", Verbose::VERBOSITY_NORMAL);
    4.  
    5.     int numTemporalKFs = 15;
    6.  
    7.     //Relationship to rebuild the essential graph, it is used two times, first in the local window and later in the rest of the map
    8.     KeyFrame* pNewChild;
    9.     KeyFrame* pNewParent;
    10.  
    11.     vector vpLocalCurrentWindowKFs;
    12.     vector vpMergeConnectedKFs;
    13.  
    14.     // Flag that is true only when we stopped a running BA, in this case we need relaunch at the end of the merge
    15.     bool bRelaunchBA = false;
    16.  
    17.     Verbose::PrintMess("MERGE: Check Full Bundle Adjustment", Verbose::VERBOSITY_DEBUG);
    18.     // If a Global Bundle Adjustment is running, abort it
    19.     if(isRunningGBA())
    20.     {
    21.         unique_lock lock(mMutexGBA);
    22.         mbStopGBA = true;
    23.  
    24.         mnFullBAIdx++;
    25.  
    26.         if(mpThreadGBA)
    27.         {
    28.             mpThreadGBA->detach();
    29.             delete mpThreadGBA;
    30.         }
    31.         bRelaunchBA = true;
    32.     }
    33.  
    34.     Verbose::PrintMess("MERGE: Request Stop Local Mapping", Verbose::VERBOSITY_DEBUG);
    35.     mpLocalMapper->RequestStop();
    36.     // Wait until Local Mapping has effectively stopped
    37.     while(!mpLocalMapper->isStopped())
    38.     {
    39.         usleep(1000);
    40.     }
    41.     Verbose::PrintMess("MERGE: Local Map stopped", Verbose::VERBOSITY_DEBUG);
    42.  
    43.     mpLocalMapper->EmptyQueue();
    44.  
    45.     // Merge map will become in the new active map with the local window of KFs and MPs from the current map.
    46.     // Later, the elements of the current map will be transform to the new active map reference, in order to keep real time tracking
    47.     Map* pCurrentMap = mpCurrentKF->GetMap();
    48.     Map* pMergeMap = mpMergeMatchedKF->GetMap();
    49.  
    50.     // Ensure current keyframe is updated
    51.     mpCurrentKF->UpdateConnections();
    52.  
    53.     //Get the current KF and its neighbors(visual->covisibles; inertial->temporal+covisibles)
    54.     set spLocalWindowKFs;
    55.     //Get MPs in the welding area from the current map
    56.     set spLocalWindowMPs;
    57.     if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization
    58.     {
    59.         KeyFrame* pKFi = mpCurrentKF;
    60.         int nInserted = 0;
    61.         while(pKFi && nInserted < numTemporalKFs)
    62.         {
    63.             spLocalWindowKFs.insert(pKFi);
    64.             pKFi = mpCurrentKF->mPrevKF;
    65.             nInserted++;
    66.  
    67.             set spMPi = pKFi->GetMapPoints();
    68.             spLocalWindowMPs.insert(spMPi.begin(), spMPi.end());
    69.         }
    70.  
    71.         pKFi = mpCurrentKF->mNextKF;
    72.         while(pKFi)
    73.         {
    74.             spLocalWindowKFs.insert(pKFi);
    75.  
    76.             set spMPi = pKFi->GetMapPoints();
    77.             spLocalWindowMPs.insert(spMPi.begin(), spMPi.end());
    78.         }
    79.     }
    80.     else
    81.     {
    82.         spLocalWindowKFs.insert(mpCurrentKF);
    83.     }
    84.  
    85.     vector vpCovisibleKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(numTemporalKFs);
    86.     spLocalWindowKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end());
    87.     const int nMaxTries = 3;
    88.     int nNumTries = 0;
    89.     while(spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries)
    90.     {
    91.         vector vpNewCovKFs;
    92.         vpNewCovKFs.empty();
    93.         for(KeyFrame* pKFi : spLocalWindowKFs)
    94.         {
    95.             vector vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2);
    96.             for(KeyFrame* pKFcov : vpKFiCov)
    97.             {
    98.                 if(pKFcov && !pKFcov->isBad() && spLocalWindowKFs.find(pKFcov) == spLocalWindowKFs.end())
    99.                 {
    100.                     vpNewCovKFs.push_back(pKFcov);
    101.                 }
    102.  
    103.             }
    104.         }
    105.  
    106.         spLocalWindowKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end());
    107.         nNumTries++;
    108.     }
    109.  
    110. 	 //TODO TEST
    111.     //vector vpTestKFs = pCurrentMap->GetAllKeyFrames();
    112.     //spLocalWindowKFs.insert(vpTestKFs.begin(), vpTestKFs.end());
    113.     for(KeyFrame* pKFi : spLocalWindowKFs)
    114.     {
    115.         if(!pKFi || pKFi->isBad())
    116.             continue;
    117.  
    118.         set spMPs = pKFi->GetMapPoints();
    119.         spLocalWindowMPs.insert(spMPs.begin(), spMPs.end());
    120.     }
    121.  
    122.     set spMergeConnectedKFs;
    123.     if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization
    124.     {
    125.         KeyFrame* pKFi = mpMergeMatchedKF;
    126.         int nInserted = 0;
    127.         while(pKFi && nInserted < numTemporalKFs)
    128.         {
    129.             spMergeConnectedKFs.insert(pKFi);
    130.             pKFi = mpCurrentKF->mPrevKF;
    131.             nInserted++;
    132.         }
    133.  
    134.         pKFi = mpMergeMatchedKF->mNextKF;
    135.         while(pKFi)
    136.         {
    137.             spMergeConnectedKFs.insert(pKFi);
    138.         }
    139.     }
    140.     else
    141.     {
    142.         spMergeConnectedKFs.insert(mpMergeMatchedKF);
    143.     }
    144.     vpCovisibleKFs = mpMergeMatchedKF->GetBestCovisibilityKeyFrames(numTemporalKFs);
    145.     spMergeConnectedKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end());
    146.     nNumTries = 0;
    147.     while(spMergeConnectedKFs.size() < numTemporalKFs && nNumTries < nMaxTries)
    148.     {
    149.         vector vpNewCovKFs;
    150.         for(KeyFrame* pKFi : spMergeConnectedKFs)
    151.         {
    152.             vector vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2);
    153.             for(KeyFrame* pKFcov : vpKFiCov)
    154.             {
    155.                 if(pKFcov && !pKFcov->isBad() && spMergeConnectedKFs.find(pKFcov) == spMergeConnectedKFs.end())
    156.                 {
    157.                     vpNewCovKFs.push_back(pKFcov);
    158.                 }
    159.  
    160.             }
    161.         }
    162.  
    163.         spMergeConnectedKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end());
    164.         nNumTries++;
    165.     }
    166.  
    167.     set spMapPointMerge;
    168.     for(KeyFrame* pKFi : spMergeConnectedKFs)
    169.     {
    170.         set vpMPs = pKFi->GetMapPoints();
    171.         spMapPointMerge.insert(vpMPs.begin(),vpMPs.end());
    172.     }
    173.  
    174.     vector vpCheckFuseMapPoint;
    175.     vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
    176.     std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));
    177.  
    178.     cv::Mat Twc = mpCurrentKF->GetPoseInverse();
    179.  
    180.     cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
    181.     cv::Mat twc = Twc.rowRange(0,3).col(3);
    182.     g2o::Sim3 g2oNonCorrectedSwc(Converter::toMatrix3d(Rwc),Converter::toVector3d(twc),1.0);
    183.     g2o::Sim3 g2oNonCorrectedScw = g2oNonCorrectedSwc.inverse();
    184.     g2o::Sim3 g2oCorrectedScw = mg2oMergeScw;
    185.  
    186.     KeyFrameAndPose vCorrectedSim3, vNonCorrectedSim3;
    187.     vCorrectedSim3[mpCurrentKF]=g2oCorrectedScw;
    188.     vNonCorrectedSim3[mpCurrentKF]=g2oNonCorrectedScw;
    189.  
    190.     for(KeyFrame* pKFi : spLocalWindowKFs)
    191.     {
    192.         if(!pKFi || pKFi->isBad())
    193.         {
    194.             continue;
    195.         }
    196.  
    197.         g2o::Sim3 g2oCorrectedSiw;
    198.  
    199.         if(pKFi!=mpCurrentKF)
    200.         {
    201.             cv::Mat Tiw = pKFi->GetPose();
    202.             cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
    203.             cv::Mat tiw = Tiw.rowRange(0,3).col(3);
    204.             g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
    205.             //Pose without correction
    206.             vNonCorrectedSim3[pKFi]=g2oSiw;
    207.  
    208.             cv::Mat Tic = Tiw*Twc;
    209.             cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
    210.             cv::Mat tic = Tic.rowRange(0,3).col(3);
    211.             g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
    212.             g2oCorrectedSiw = g2oSic*mg2oMergeScw;
    213.             vCorrectedSim3[pKFi]=g2oCorrectedSiw;
    214.         }
    215.         else
    216.         {
    217.             g2oCorrectedSiw = g2oCorrectedScw;
    218.         }
    219.         pKFi->mTcwMerge  = pKFi->GetPose();
    220.  
    221.         // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
    222.         Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
    223.         Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
    224.         double s = g2oCorrectedSiw.scale();
    225.  
    226.         pKFi->mfScale = s;
    227.         eigt *=(1./s); //[R t/s;0 1]
    228.  
    229.         cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
    230.  
    231.         pKFi->mTcwMerge = correctedTiw;
    232. 		// Make sure connections are updated
    233.         if(pCurrentMap->isImuInitialized())
    234.         {
    235.             Eigen::Matrix3d Rcor = eigR.transpose()*vNonCorrectedSim3[pKFi].rotation().toRotationMatrix();
    236.             pKFi->mVwbMerge = Converter::toCvMat(Rcor)*pKFi->GetVelocity();
    237.         }
    238.     		//TODO DEBUG to know which are the KFs that had been moved to the other map
    239.        //pKFi->mnOriginMapId = 5;
    240.     }
    241.  
    242.     for(MapPoint* pMPi : spLocalWindowMPs)
    243.     {
    244.         if(!pMPi || pMPi->isBad())
    245.             continue;
    246.  
    247.         KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
    248.         g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse();
    249.         g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref];
    250.  
    251.         // Project with non-corrected pose and project back with corrected pose
    252.         cv::Mat P3Dw = pMPi->GetWorldPos();
    253.         Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
    254.         Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oNonCorrectedSiw.map(eigP3Dw));
    255.         Eigen::Matrix3d eigR = g2oCorrectedSwi.rotation().toRotationMatrix();
    256.         Eigen::Matrix3d Rcor = eigR * g2oNonCorrectedSiw.rotation().toRotationMatrix();
    257.  
    258.         cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
    259.  
    260.         pMPi->mPosMerge = cvCorrectedP3Dw;
    261.         pMPi->mNormalVectorMerge = Converter::toCvMat(Rcor) * pMPi->GetNormal();
    262.     }
    263.  
    264.     {
    265.         unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
    266.         unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map
    267.  
    268.         for(KeyFrame* pKFi : spLocalWindowKFs)
    269.         {
    270.             if(!pKFi || pKFi->isBad())
    271.             {
    272.                 continue;
    273.             }
    274.  
    275.             pKFi->mTcwBefMerge = pKFi->GetPose();
    276.             pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
    277.             pKFi->SetPose(pKFi->mTcwMerge);
    278.  
    279.             // Make sure connections are updated
    280.             pKFi->UpdateMap(pMergeMap);
    281.             pKFi->mnMergeCorrectedForKF = mpCurrentKF->mnId;
    282.             pMergeMap->AddKeyFrame(pKFi);
    283.             pCurrentMap->EraseKeyFrame(pKFi);
    284.  
    285.             if(pCurrentMap->isImuInitialized())
    286.             {
    287.                 pKFi->SetVelocity(pKFi->mVwbMerge);
    288.             }
    289.         }
    290.  
    291.         for(MapPoint* pMPi : spLocalWindowMPs)
    292.         {
    293.             if(!pMPi || pMPi->isBad())
    294.                 continue;
    295.  
    296.             pMPi->SetWorldPos(pMPi->mPosMerge);
    297.             pMPi->SetNormalVector(pMPi->mNormalVectorMerge);
    298.             pMPi->UpdateMap(pMergeMap);
    299.             pMergeMap->AddMapPoint(pMPi);
    300.             pCurrentMap->EraseMapPoint(pMPi);
    301.         }
    302.  
    303.         mpAtlas->ChangeMap(pMergeMap);
    304.         mpAtlas->SetMapBad(pCurrentMap);
    305.         pMergeMap->IncreaseChangeIndex();
    306.     }
    307.  
    308.  
    309.     //Rebuild the essential graph in the local window
    310.     pCurrentMap->GetOriginKF()->SetFirstConnection(false);
    311.     pNewChild = mpCurrentKF->GetParent(); // Old parent, it will be the new child of this KF
    312.     pNewParent = mpCurrentKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent)
    313.     mpCurrentKF->ChangeParent(mpMergeMatchedKF);
    314.     while(pNewChild )
    315.     {
    316.         pNewChild->EraseChild(pNewParent); // We remove the relation between the old parent and the new for avoid loop
    317.         KeyFrame * pOldParent = pNewChild->GetParent();
    318.  
    319.         pNewChild->ChangeParent(pNewParent);
    320.  
    321.         pNewParent = pNewChild;
    322.         pNewChild = pOldParent;
    323.  
    324.     }
    325.  
    326.     //Update the connections between the local window
    327.     mpMergeMatchedKF->UpdateConnections();
    328.  
    329.     vpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
    330.     vpMergeConnectedKFs.push_back(mpMergeMatchedKF);
    331.     vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
    332.     std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));
    333.  
    334.     // Project MapPoints observed in the neighborhood of the merge keyframe
    335.     // into the current keyframe and neighbors using corrected poses.
    336.     // Fuse duplications.
    337.     SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint);
    338.  
    339.     // Update connectivity
    340.     for(KeyFrame* pKFi : spLocalWindowKFs)
    341.     {
    342.         if(!pKFi || pKFi->isBad())
    343.             continue;
    344.  
    345.         pKFi->UpdateConnections();
    346.     }
    347.     for(KeyFrame* pKFi : spMergeConnectedKFs)
    348.     {
    349.         if(!pKFi || pKFi->isBad())
    350.             continue;
    351.  
    352.         pKFi->UpdateConnections();
    353.     }
    354.  
    355.     bool bStop = false;
    356.     Verbose::PrintMess("MERGE: Start local BA ", Verbose::VERBOSITY_DEBUG);
    357.     vpLocalCurrentWindowKFs.clear();
    358.     vpMergeConnectedKFs.clear();
    359.     std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs));
    360.     std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs));
    361.     if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO)
    362.     {
    363.         Optimizer::MergeInertialBA(mpLocalMapper->GetCurrKF(),mpMergeMatchedKF,&bStop, mpCurrentKF->GetMap(),vCorrectedSim3);
    364.     }
    365.     else
    366.     {
    367.         Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop);
    368.     }
    369.  
    370.     // Loop closed. Release Local Mapping.
    371.     mpLocalMapper->Release();
    372.  
    373.     Verbose::PrintMess("MERGE: Finish the LBA", Verbose::VERBOSITY_DEBUG);
    374.  
    375.  
    376.     
    377.     //Update the non critical area from the current map to the merged map
    378.     vector vpCurrentMapKFs = pCurrentMap->GetAllKeyFrames();
    379.     vector vpCurrentMapMPs = pCurrentMap->GetAllMapPoints();
    380.  
    381.     if(vpCurrentMapKFs.size() == 0)
    382.     {
    383.         Verbose::PrintMess("MERGE: There are not KFs outside of the welding area", Verbose::VERBOSITY_DEBUG);
    384.     }
    385.     else
    386.     {
    387.         Verbose::PrintMess("MERGE: Calculate the new position of the elements outside of the window", Verbose::VERBOSITY_DEBUG);
    388.         //Apply the transformation
    389.         {
    390.             if(mpTracker->mSensor == System::MONOCULAR)
    391.             {
    392.                 unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
    393.  
    394.                 for(KeyFrame* pKFi : vpCurrentMapKFs)
    395.                 {
    396.                     if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
    397.                     {
    398.                         continue;
    399.                     }
    400.  
    401.                     g2o::Sim3 g2oCorrectedSiw;
    402.  
    403.                     cv::Mat Tiw = pKFi->GetPose();
    404.                     cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
    405.                     cv::Mat tiw = Tiw.rowRange(0,3).col(3);
    406.                     g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
    407.                     //Pose without correction
    408.                     vNonCorrectedSim3[pKFi]=g2oSiw;
    409.  
    410.                     cv::Mat Tic = Tiw*Twc;
    411.                     cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
    412.                     cv::Mat tic = Tic.rowRange(0,3).col(3);
    413.                     g2o::Sim3 g2oSim(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
    414.                     g2oCorrectedSiw = g2oSim*mg2oMergeScw;
    415.                     vCorrectedSim3[pKFi]=g2oCorrectedSiw;
    416.  
    417.                     // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
    418.                     Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
    419.                     Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
    420.                     double s = g2oCorrectedSiw.scale();
    421.  
    422.                     pKFi->mfScale = s;
    423.                     eigt *=(1./s); //[R t/s;0 1]
    424.  
    425.                     cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
    426.  
    427.                     pKFi->mTcwBefMerge = pKFi->GetPose();
    428.                     pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
    429.  
    430.                     pKFi->SetPose(correctedTiw);
    431.  
    432.                     if(pCurrentMap->isImuInitialized())
    433.                     {
    434.                         Eigen::Matrix3d Rcor = eigR.transpose()*vNonCorrectedSim3[pKFi].rotation().toRotationMatrix();
    435.                         pKFi->SetVelocity(Converter::toCvMat(Rcor)*pKFi->GetVelocity()); // TODO: should add here scale s
    436.                     }
    437.  
    438.                 }
    439.                 for(MapPoint* pMPi : vpCurrentMapMPs)
    440.                 {
    441.                     if(!pMPi || pMPi->isBad()|| pMPi->GetMap() != pCurrentMap)
    442.                         continue;
    443.  
    444.                     KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
    445.                     g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse();
    446.                     g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref];
    447.  
    448.                     // Project with non-corrected pose and project back with corrected pose
    449.                     cv::Mat P3Dw = pMPi->GetWorldPos();
    450.                     Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
    451.                     Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oNonCorrectedSiw.map(eigP3Dw));
    452.  
    453.                     cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
    454.                     pMPi->SetWorldPos(cvCorrectedP3Dw);
    455.  
    456.                     pMPi->UpdateNormalAndDepth();
    457.                 }
    458.             }
    459.         }
    460.  
    461.         mpLocalMapper->RequestStop();
    462.         // Wait until Local Mapping has effectively stopped
    463.         while(!mpLocalMapper->isStopped())
    464.         {
    465.             usleep(1000);
    466.         }
    467.  
    468.         // Optimize graph (and update the loop position for each element form the begining to the end)
    469.         if(mpTracker->mSensor != System::MONOCULAR)
    470.         {
    471.             Optimizer::OptimizeEssentialGraph(mpCurrentKF, vpMergeConnectedKFs, vpLocalCurrentWindowKFs, vpCurrentMapKFs, vpCurrentMapMPs);
    472.         }
    473.  
    474.  
    475.         {
    476.             // Get Merge Map Mutex
    477.             unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
    478.             unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map
    479.  
    480.             for(KeyFrame* pKFi : vpCurrentMapKFs)
    481.             {
    482.                 if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
    483.                 {
    484.                     continue;
    485.                 }
    486.  
    487.                 // Make sure connections are updated
    488.                 pKFi->UpdateMap(pMergeMap);
    489.                 pMergeMap->AddKeyFrame(pKFi);
    490.                 pCurrentMap->EraseKeyFrame(pKFi);
    491.             }
    492.  
    493.             for(MapPoint* pMPi : vpCurrentMapMPs)
    494.             {
    495.                 if(!pMPi || pMPi->isBad())
    496.                     continue;
    497.  
    498.                 pMPi->UpdateMap(pMergeMap);
    499.                 pMergeMap->AddMapPoint(pMPi);
    500.                 pCurrentMap->EraseMapPoint(pMPi);
    501.             }
    502.         }
    503.     }
    504.  
    505.     mpLocalMapper->Release();
    506.  
    507.     Verbose::PrintMess("MERGE:Completed!!!!!", Verbose::VERBOSITY_DEBUG);
    508.  
    509.     if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1)))
    510.     {
    511.         // Launch a new thread to perform Global Bundle Adjustment
    512.         Verbose::PrintMess("Relaunch Global BA", Verbose::VERBOSITY_DEBUG);
    513.         mbRunningGBA = true;
    514.         mbFinishedGBA = false;
    515.         mbStopGBA = false;
    516.         mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this, pMergeMap, mpCurrentKF->mnId);
    517.     }
    518.  
    519.     mpMergeMatchedKF->AddMergeEdge(mpCurrentKF);
    520.     mpCurrentKF->AddMergeEdge(mpMergeMatchedKF);
    521.  
    522.     pCurrentMap->IncreaseChangeIndex();
    523.     pMergeMap->IncreaseChangeIndex();
    524.  
    525.     mpAtlas->RemoveBadMaps();
    526.  
    527. }

    参考文献

    ORB SLAM2源码解读(十一)：LoopClosing类 - 知乎

    https://blog.csdn.net/qq_45794281/article/details/121579998 

  • 相关阅读:
    光流法optical flow
    基于MiniTest的小程序自动化测试
    (Git)git clone报错——SSL certificate problem: self signed certificate
    2022年数学建模国赛--赛后总结
    string（讲解）
    ffmpeg基础四：RTP协议
    【历史上的今天】8 月 15 日：苹果推出初代 iMac；谷歌收购摩托罗拉移动；Fuchsia 首次发布
    数据安全：.[bkpsvr@firemail.cc].EKING勒索病毒的特点和数据恢复方法
    Java多线程篇(3)——线程池
    企业集中监控体系思路及架构
  • 原文地址：https://blog.csdn.net/xhtchina/article/details/119767848