目录
3.1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来
3.3 如果检测到融合（当前关键帧与其他地图有关联）, 则合并地图
3.3.2 纯视觉融合 LoopClosing::MergeLocal
3.4 回环矫正（如果(没有检测到融合)检测到回环，则回环矫正）
1.闭环及地图合并线程的目的和意义
1.建立更多的中长期数据关联（距离很远的数据帧的数据关联）。寻找闭环/融合候选关键帧、窗口内welding BA、本质图BA、全局BA。
2.可以将多个子地图连接成一个精确的全局地图。
3.极大的降低整体的位姿和地图点误差，从而获得全局一致的地图和准确的位姿估计。
闭环和地图合并示意图 我们沿着圆圈走了一圈到了红色的帧的位置（本来应该回到虚线的原点，但因为累计误差回不到了），我们要做的就是找到当前关键帧pKF和候选关键帧组（绿色的帧）（可以是闭环候选-位于一个地图，融合候选-位于不同地图），通过DBoW匹配。
窗口内welding BA：在当前关键帧画了一个窗口，候选关键帧也开了一个窗口，将两个窗口闭在一起。
2.闭环及地图合并流程
对比ORB-SLAM2和ORB-SLAM3闭环线程：
while(1) { // 检查是否有新关键帧 if(CheckNewKeyFrames()) { // 检测闭环候选关键帧和共视连续性 if(DetectLoop()) { // 计算Sim3变换[sR|t],在双目/RGBD模式下s=1 if(ComputeSim3()) { // 闭环矫正融合及位姿图优化 CorrectLoop(); } } } }void LoopClosing::Run() { mbFinished =false; // 线程主循环 while(1) { //NEW LOOP AND MERGE DETECTION ALGORITHM //---------------------------- // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的 // 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue // Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来 if(CheckNewKeyFrames()) { // 这部分后续未使用 if(mpLastCurrentKF) { mpLastCurrentKF->mvpLoopCandKFs.clear(); mpLastCurrentKF->mvpMergeCandKFs.clear(); } #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartPR = std::chrono::steady_clock::now(); #endif // Step 2 检测有没有共视的区域 bool bFindedRegion = NewDetectCommonRegions(); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndPR = std::chrono::steady_clock::now(); double timePRTotal = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPR - time_StartPR).count(); vdPRTotal_ms.push_back(timePRTotal); #endif if(bFindedRegion) { // Step 3 如果检测到融合（当前关键帧与其他地图有关联）, 则合并地图 if(mbMergeDetected) { // 在imu没有初始化就放弃融合 if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) && (!mpCurrentKF->GetMap()->isImuInitialized())) { cout << "IMU is not initilized, merge is aborted" << endl; } else { // 拿到融合帧在自己地图所在坐标系(w2)下的位姿 Sophus::SE3d mTmw = mpMergeMatchedKF->GetPose().cast<double>(); g2o::Sim3 gSmw2(mTmw.unit_quaternion(), mTmw.translation(), 1.0); // 拿到当前帧在自己地图所在坐标系(w1)下的位姿 Sophus::SE3d mTcw = mpCurrentKF->GetPose().cast<double>(); g2o::Sim3 gScw1(mTcw.unit_quaternion(), mTcw.translation(), 1.0); // 根据共同区域检测时的Sim3结果得到当前帧在w2下的位姿 // mg2oMergeSlw 里存放的是融合候选关键帧所在的世界坐标系w2到当前帧的Sim3位姿 // l = c , w2是融合候选关键帧所在的世界坐标系 g2o::Sim3 gSw2c = mg2oMergeSlw.inverse(); // 这个没有用到 : 融合帧在w1下的位姿 g2o::Sim3 gSw1m = mg2oMergeSlw; // 记录焊接变换(Sim3) T_w2_w1 , 这个量实际是两个地图坐标系的关系 T_w2_w1 = T_w2_c * T_c_w1 mSold_new = (gSw2c * gScw1); // 如果是imu模式 if(mpCurrentKF->GetMap()->IsInertial() && mpMergeMatchedKF->GetMap()->IsInertial()) { cout << "Merge check transformation with IMU" << endl; // 如果尺度变换太大, 认为累积误差较大，则放弃融合 if(mSold_new.scale()<0.90||mSold_new.scale()>1.1){ mpMergeLastCurrentKF->SetErase(); mpMergeMatchedKF->SetErase(); mnMergeNumCoincidences = 0; mvpMergeMatchedMPs.clear(); mvpMergeMPs.clear(); mnMergeNumNotFound = 0; mbMergeDetected = false; Verbose::PrintMess("scale bad estimated. Abort merging", Verbose::VERBOSITY_NORMAL); continue; } // If inertial, force only yaw // 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0 // 通过物理约束来保证两个坐标轴都是水平的 if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) && mpCurrentKF->GetMap()->GetIniertialBA1()) { Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix()); phi(0)=0; phi(1)=0; mSold_new = g2o::Sim3(ExpSO3(phi),mSold_new.translation(),1.0); } } // 这个变量没有用到 mg2oMergeSmw = gSmw2 * gSw2c * gScw1; // 更新mg2oMergeScw mg2oMergeScw = mg2oMergeSlw; //mpTracker->SetStepByStep(true); Verbose::PrintMess("*Merge detected", Verbose::VERBOSITY_QUIET); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartMerge = std::chrono::steady_clock::now(); nMerges += 1; #endif // TODO UNCOMMENT // 如果是imu模式,则开启 Visual-Inertial Map Merging // 如果是纯视觉模式,则开启 Visual-Welding Map Merging if (mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) MergeLocal2(); else MergeLocal(); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndMerge = std::chrono::steady_clock::now(); double timeMergeTotal = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndMerge - time_StartMerge).count(); vdMergeTotal_ms.push_back(timeMergeTotal); #endif Verbose::PrintMess("Merge finished!", Verbose::VERBOSITY_QUIET); } // 记录时间戳 vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp); vdPR_MatchedTime.push_back(mpMergeMatchedKF->mTimeStamp); // 标记Place recognition结果为地图融合 vnPR_TypeRecogn.push_back(1); // Reset all variables // 重置所有融合相关变量 mpMergeLastCurrentKF->SetErase(); mpMergeMatchedKF->SetErase(); mnMergeNumCoincidences = 0; mvpMergeMatchedMPs.clear(); mvpMergeMPs.clear(); mnMergeNumNotFound = 0; mbMergeDetected = false; // 重置所有回环相关变量, 说明对与当前帧同时有回环和融合的情况只进行融合 if(mbLoopDetected) { // Reset Loop variables mpLoopLastCurrentKF->SetErase(); mpLoopMatchedKF->SetErase(); mnLoopNumCoincidences = 0; mvpLoopMatchedMPs.clear(); mvpLoopMPs.clear(); mnLoopNumNotFound = 0; mbLoopDetected = false; } } // Step 4 如果(没有检测到融合)检测到回环, 则回环矫正 if(mbLoopDetected) { // 标记时间戳 bool bGoodLoop = true; vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp); vdPR_MatchedTime.push_back(mpLoopMatchedKF->mTimeStamp); vnPR_TypeRecogn.push_back(0); Verbose::PrintMess("*Loop detected", Verbose::VERBOSITY_QUIET); // 更新 mg2oLoopScw mg2oLoopScw = mg2oLoopSlw; //*mvg2oSim3LoopTcw[nCurrentIndex]; // 如果是带imu的模式则做下判断，纯视觉跳过 if(mpCurrentKF->GetMap()->IsInertial()) { // 拿到当前关键帧相对于世界坐标系的位姿 Sophus::SE3d Twc = mpCurrentKF->GetPoseInverse().cast<double>(); g2o::Sim3 g2oTwc(Twc.unit_quaternion(),Twc.translation(),1.0); // mg2oLoopScw是通过回环检测的Sim3计算出的回环矫正后的当前关键帧的初始位姿, Twc是当前关键帧回环矫正前的位姿. // g2oSww_new 可以理解为correction g2o::Sim3 g2oSww_new = g2oTwc*mg2oLoopScw; // 拿到 roll ,pitch ,yaw Eigen::Vector3d phi = LogSO3(g2oSww_new.rotation().toRotationMatrix()); cout << "phi = " << phi.transpose() << endl; // 这里算是通过imu重力方向验证回环结果, 如果pitch或roll角度偏差稍微有一点大,则回环失败. 对yaw容忍比较大(20度) if (fabs(phi(0))<0.008f && fabs(phi(1))<0.008f && fabs(phi(2))<0.349f) { // 如果是imu模式 if(mpCurrentKF->GetMap()->IsInertial()) { // If inertial, force only yaw // 如果是imu模式,强制将焊接变换的的 roll 和 pitch 设为0
最低0.47元/天 解锁文章
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
