本文详细解析了ORB-SLAM3中的LoopClosing::MergeLocal函数，该函数用于纯视觉模式下的地图融合。在地图融合过程中，包括停止全局BA和局部建图线程、构建局部窗口、记录矫正前后值、地图替换、生成树融合、地图点融合、更新关键帧图关系、局部BA和剩余地图优化等步骤，确保地图一致性与实时性。
目录
4.2 构建当前关键帧和融合关键帧的局部窗口(关键帧+地图点)
4.3 根据之前的Sim3初始值，记录当前帧窗口内关键帧，地图点的矫正前的值，和矫正后的初始值
4.4 两个地图以新（当前帧所在地图）换旧（融合帧所在地图），包括关键帧及地图点关联地图的以新换旧、地图集的以新换旧
4.6 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里,把重复的点融合掉(以旧换新)
4.7 因为融合导致地图点变化，需要更新关键帧中图的连接关系
4.9 在当前帧整个剩下的地图中（局部窗口外，认为没那么紧急处理）对位姿和地图点进行矫正传播并进行本质图优化
1.函数作用
纯视觉地图融合，在检测到成功验证的融合帧后进行。
2.函数流程
特点：
融合是在不同的地图中进行操作，而ORB-SLAM2中闭环只有一个地图，因为融合的地图可能比较大，为了提高实时性。在进行完局部窗口的welding BA后，就开启了local mapping线程。
之后再进行剩下的地图中(局部窗口外，认为没那么紧急处理)对位姿和地图点进行矫正传播。然后进行本质图优化。多了一个融合生成树的操作。
步骤
1. 停止全局BA和局部建图线程
2. 构建当前关键帧和融合关键帧的局部窗口，局部窗口包括一级相邻和二级相邻共视关键帧(总共15个)，以及它们的地图点。
3. 根据之前的Sim3初始值，记录当前帧窗口内关键帧、地图点的矫正前的值(vNonCorrectedSim3、eigP3Dw)，和矫正后的初始值(vCorrectedSim3、cvCorrectedP3Dw)
4. 两个地图以新(当前帧所在地图)换旧(融合帧所在地图)，包括关键帧及地图点关联地图的以新换旧、地图集的以新换旧。
5. 融合新旧地图的生成树。
6. 把融合关键帧的共视窗口里的地图点投到当前关键帧的共视窗口里，把重复的点融合掉(以旧换新)
7. 因为融合导致地图点变化。需要更新关键帧中图的连接关系
8. 在缝合(Welding)区域进行local BA(论文中称为Welding BA)。优化当前关键帧共视窗口里的所有关键帧和地图点，固定所有融合帧共视窗口里的关键帧。
9. 在当前帧整个剩下的地图中对位姿和地图点进行矫正传播并进行本质图优化(没那么紧急，可以晚点做)。优化的对象是当前关键帧所在地图里的所有关键帧(除了当前关键帧共视窗口内的关键帧) + 当前地图里的所有地图点，固定不优化的是所有融合帧共视窗口内的关
键帧 + 所有当前关键帧共视窗口内的关键帧。
10. 如果需要的话，进行全局BA。
3.代码注释
/** * @brief 纯视觉地图融合。在检测到成功验证的融合帧后进行 * 1. 焊缝区域局部BA * 2. Essential Graph BA * 融合两张地图为一张地图 */ void LoopClosing::MergeLocal() { // 窗口内共视关键帧的数量， 上个0.4版本是15 int numTemporalKFs = 25; //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 // 建立本质图必须的量 KeyFrame* pNewChild; KeyFrame* pNewParent; // 当前关键帧的窗口 vector<KeyFrame*> vpLocalCurrentWindowKFs; // 候选关键帧的窗口 vector<KeyFrame*> vpMergeConnectedKFs; // 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; //Verbose::PrintMess("MERGE-VISUAL: Check Full Bundle Adjustment", Verbose::VERBOSITY_DEBUG); // If a Global Bundle Adjustment is running, abort it if(isRunningGBA()) { unique_lock<mutex> lock(mMutexGBA); mbStopGBA = true; mnFullBAIdx++; if(mpThreadGBA) { mpThreadGBA->detach(); delete mpThreadGBA; } bRelaunchBA = true; // 以后还会重新开启 } //Verbose::PrintMess("MERGE-VISUAL: Request Stop Local Mapping", Verbose::VERBOSITY_DEBUG); //cout << "Request Stop Local Mapping" << endl; // 请求局部建图线程停止 mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped // 等待局部建图工作停止 while(!mpLocalMapper->isStopped()) { usleep(1000); } //cout << "Local Map stopped" << endl; mpLocalMapper->EmptyQueue(); // Merge map will become in the new active map with the local window of KFs and MPs from the current map. // Later, the elements of the current map will be transform to the new active map reference, in order to keep real time tracking // 当前关键帧的共视关键帧和他们观测到的地图点会先被融合, 融合后的图会变成新的当前激活地图. // 之后, 所有当前地图的其他部分会被转换到当前地图中, 这样是为了保证tracking的实时性. // 当前关键帧地图的指针 Map* pCurrentMap = mpCurrentKF->GetMap(); // 融合关键帧地图的指针 Map* pMergeMap = mpMergeMatchedKF->GetMap(); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartMerge = std::chrono::steady_clock::now(); #endif // Ensure current keyframe is updated // 先保证当前关键帧的链接关系是最新的 mpCurrentKF->UpdateConnections(); // Step 1 构建当前关键帧和融合关键帧的局部窗口(关键帧+地图点) //Get the current KF and its neighbors(visual->covisibles; inertial->temporal+covisibles) // 当前关键帧的局部窗口(仅是辅助容器,防止重复添加) set<KeyFrame*> spLocalWindowKFs; //Get MPs in the welding area from the current map // 当前关键帧局部串口能观测到的所有地图点(仅是辅助容器,防止重复添加) set<MapPoint*> spLocalWindowMPs; // 这段代码只在visual状态下才会被使用，所以只会执行else // Step 1.1 构造当前关键帧局部共视帧窗口 if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO 纯视觉模式,不执行 { KeyFrame* pKFi = mpCurrentKF; int nInserted = 0; while(pKFi && nInserted < numTemporalKFs) { spLocalWindowKFs.insert(pKFi); pKFi = mpCurrentKF->mPrevKF; nInserted++; set<MapPoint*> spMPi = pKFi->GetMapPoints(); spLocalWindowMPs.insert(spMPi.begin(), spMPi.end()); } pKFi = mpCurrentKF->mNextKF; while(pKFi) //! 这里会死循环,不过无所谓，这个外面的if永远不会执行 { spLocalWindowKFs.insert(pKFi); set<MapPoint*> spMPi = pKFi->GetMapPoints(); spLocalWindowMPs.insert(spMPi.begin(), spMPi.end()); pKFi = mpCurrentKF->mNextKF; } } else { // 把自己先加到窗口内 spLocalWindowKFs.insert(mpCurrentKF); } // 拿到当前关键帧的numTemporalKFs(25)个最佳共视关键帧 vector<KeyFrame*> vpCovisibleKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(numTemporalKFs); // 把当前帧的共视帧都加到窗口里 spLocalWindowKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end()); // 1.0版本新加的，将当前关键帧也添加进来 spLocalWindowKFs.insert(mpCurrentKF); // 限制while循环次数 0.4版本是3 const int nMaxTries = 5; int nNumTries = 0; // 如果窗口里的关键帧数量不够就再拉一些窗口里的关键帧的共视关键帧(二级共视关键帧)进来 // while结束后就把当前关键帧mpCurrentKF的25个共视关键帧找到了,存放在spLocalWindowKFs变量中 while(spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries) { vector<KeyFrame*> vpNewCovKFs; vpNewCovKFs.empty(); // 遍历创口内的每一个关键帧 for(KeyFrame* pKFi : spLocalWindowKFs) { // 拿到一些二级共视关键帧 vector<KeyFrame*> vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2); // 对于每个二级共视关键帧 for(KeyFrame* pKFcov : vpKFiCov) { // 如果指针不为空,且关键帧没有被标记为bad,且没有被添加过则加到窗口内 if(pKFcov && !pKFcov->isBad() && spLocalWindowKFs.find(pKFcov) == spLocalWindowKFs.end()) { vpNewCovKFs.push_back(pKFcov); } } } // 用set记录,防止重复添加 spLocalWindowKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end()); // 递增循环次数 nNumTries++; } // Step 1.2 把当前25个共视关键帧窗口里关键帧观测到的地图点加进来放入spLocalWindowMPs中 for(KeyFrame* pKFi : spLocalWindowKFs) { if(!pKFi || pKFi->isBad()) continue; set<MapPoint*> spMPs = pKFi->GetMapPoints(); spLocalWindowMPs.insert(spMPs.begin(), spMPs.end()); } //std::cout << "[Merge]: Ma = " << to_string(pCurrentMap->GetId()) << "; #KFs = " << to_string(spLocalWindowKFs.size()) << "; #MPs = " << to_string(spLocalWindowMPs.size()) << std::endl; // Step 1.3 构造融合帧的共视帧窗口 // 融合关键帧的共视关键帧们 set<KeyFrame*> spMergeConnectedKFs; // 这段代码只在visual状态下才会被使用，所以只会执行else if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization { KeyFrame* pKFi = mpMergeMatchedKF; int nInserted = 0; while(pKFi && nInserted < numTemporalKFs/2) { spMergeConnectedKFs.insert(pKFi); pKFi = mpCurrentKF->mPrevKF; nInserted++; } pKFi = mpMergeMatchedKF->mNextKF; while(pKFi && nInserted < numTemporalKFs) { spMergeConnectedKFs.insert(pKFi); pKFi = mpCurrentKF->mNextKF; } } else { // 先把融合关键帧自己添加到窗口内 spMergeConnectedKFs.insert(mpMergeMatchedKF); } // 拿到融合关键帧最好的numTemporalKFs(25)个最佳共视关键帧 vpCovisibleKFs = mpMergeMatchedKF->GetBestCovisibilityKeyFrames(numTemporalKFs); spMergeConnectedKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end()); spMergeConnectedKFs.insert(mpMergeMatchedKF); // 记录循环次数 nNumTries = 0; // 如果融合关键帧窗口里的关键帧不够就再拉一些窗口里的关键帧的共视帧进来(二级共视关键帧) while(spMergeConnectedKFs.size() < numTemporalKFs && nNumTries < nMaxTries) { vector<KeyFrame*> vpNewCovKFs; // 遍历融合关键帧内的所有的一级共视关键帧 for(KeyFrame* pKFi : spMergeConnectedKFs) { // 拿到一些二级共视关键帧 vector<KeyFrame*> vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2); // 对于每个二级共视关键帧 for(KeyFrame* pKFcov : vpKFiCov) { // 如果指针不为空,且关键帧没有被标记为bad,且没有被添加过则加到窗口内 if(pKFcov && !pKFcov->isBad() && spMergeConnectedKFs.find(pKFcov) == spMergeConnectedKFs.end()) { vpNewCovKFs.push_back(pKFcov); } } } // 用set记录,防止重复添加 spMergeConnectedKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end()); // 递增循环次数 nNumTries++; } // Step 1.4 把融合关键帧窗口里关键帧观测到的地图点加进来 // 融合关键帧共视窗口内的所有地图点 set<MapPoint*> spMapPointMerge; for(KeyFrame* pKFi : spMergeConnectedKFs) { set<MapPoint*> vpMPs = pKFi->GetMapPoints(); spMapPointMerge.insert(vpMPs.begin(),vpMPs.end()); } // 把融合关键帧窗口内的地图点加到待融合的向量中 vector<MapPoint*> vpCheckFuseMapPoint; vpCheckFuseMapPoint.reserve(spMapPointMerge.size()); // 把spMapPointMerge拷贝到vpCheckFuseMapPoint里 std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint)); //std::cout << "[Merge]: Mm = " << to_string(pMergeMap->GetId()) << "; #KFs = " << to_string(spMergeConnectedKFs.size()) << "; #MPs = " << to_string(spMapPointMerge.size()) << std::endl; // Step 2 根据之前的Sim3初始值, 记录当前帧窗口内关键帧,地图点的矫正前的值,和矫正后的初始值 // Step 2.1 先计算当前关键帧矫正前的值,和矫正后的初始值 // 矫正前的位姿 Sophus::SE3d Twc = mpCurrentKF->GetPoseInverse().cast<double>(); // 记录没有融合矫正之前的Swc和Scw g2o::Sim3 g2oNonCorrectedSwc(Twc.unit_quaternion(),Twc.translation(),1.0); g2o::Sim3 g2oNonCorrectedScw = g2oNonCorrectedSwc.inverse(); // 拿到之前通过Sim3(见 NewDetectCommonRegion)计算得到的当前关键帧融合矫正之后的初始位姿 // mg2oMergeScw存放的是融合关键帧所在的世界坐标系到当前帧的Sim3位姿 g2o::Sim3 g2oCorrectedScw = mg2oMergeScw; //TODO Check the transformation // 记录当前关键帧窗口内所有关键帧融合矫正之前的位姿,和融合矫正之后的初始位姿 KeyFrameAndPose vCorrectedSim3, vNonCorrectedSim3; // g2oNonCorrectedScw 是当前关键帧世界坐标系下的 // g2oCorrectedScw 是融合关键帧所在的世界坐标系下的 vCorrectedSim3[mpCurrentKF]=g2oCorrectedScw; vNonCorrectedSim3[mpCurrentKF]=g2oNonCorrectedScw; #ifdef REGISTER_TIMES vnMergeKFs.push_back(spLocalWindowKFs.size() + spMergeConnectedKFs.size()); vnMergeMPs.push_back(spLocalWindowMPs.size() + spMapPointMerge.size()); #endif // Step 2.2 通过当前关键帧融合矫正前后的位姿,把当前关键帧的共视窗口里面剩余的关键帧矫正前后的位姿都给填写上 // 对于每个当前关键帧共视窗口里的关键帧 for(KeyFrame* pKFi : spLocalWindowKFs) { // 跳过空的指针,或者坏的关键帧 if(!pKFi || pKFi->isBad()) { Verbose::PrintMess("Bad KF in correction", Verbose::VERBOSITY_DEBUG); continue; } if(pKFi->GetMap() != pCurrentMap) Verbose::PrintMess("Other map KF, this should't happen", Verbose::VERBOSITY_DEBUG); // 确保这些共视关键帧在当前地图下 // 保存第i个共视关键帧融合矫正后的初始位姿 g2o::Sim3 g2oCorrectedSiw; if(pKFi!=mpCurrentKF) { // 先记录下融合矫正前的位姿 Sophus::SE3d Tiw = (pKFi->GetPose()).cast<double>(); // world 2 pKFi g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0); //Pose without correction // 在当前世界坐标系(世界坐标系1)下的位姿 vNonCorrectedSim3[pKFi]=g2oSiw; // 根据链式法则,利用当前关键帧和第i个共视关键帧的位姿关系,以及当前关键帧矫正后的初始位姿,推得第i个共视关键帧的矫正后的初始位姿 // mpCurrentKF 2 pKFi Sophus::SE3d Tic = Tiw*Twc; g2o::Sim3 g2oSic(Tic.unit_quaternion(),Tic.translation(),1.0); // mg2oMergeScw是我们在前面求出来的焊接变换(融合关键帧向当前帧mpCurrentKF的位姿变换) g2oCorrectedSiw = g2oSic*mg2oMergeScw; vCorrectedSim3[pKFi]=g2oCorrectedSiw; } else { g2oCorrectedSiw = g2oCorrectedScw; } // 这一句没有什么作用,下面又被覆盖了 pKFi->mTcwMerge = pKFi->GetPose(); // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation) // 根据上面计算得到的融合矫正后的初始位姿(Sim3),更新窗口内每个关键帧的mTcwMerge(矫正后的初始位姿) double s = g2oCorrectedSiw.scale(); pKFi->mfScale = s; // s*Riw * Pw + tiw = Pi 此时Pi在i坐标系下的坐标，尺度保留的是原来的 // Riw * Pw + tiw/s = Pi/s 此时Pi/s在i坐标系下的坐标，尺度是最新的的，所以Rt要这么保留 // 在世界坐标系2下的坐标 Sophus::SE3d correctedTiw(g2oCorrectedSiw.rotation(), g2oCorrectedSiw.translation() / s); // 修正尺度 // 赋值得到的矫正后的初始位姿 pKFi->mTcwMerge = correctedTiw.cast<float>(); // !纯视觉模式，以下代码不执行 if(pCurrentMap->isImuInitialized()) { Eigen::Quaternionf Rcor = (g2oCorrectedSiw.rotation().inverse() * vNonCorrectedSim3[pKFi].rotation()).cast<float>(); pKFi->mVwbMerge = Rcor * pKFi->GetVelocity(); } //TODO DEBUG to know which are the KFs that had been
最低0.47元/天 解锁文章
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
