diff --git a/README.md b/README.md index a6b0971..b0cf595 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ ![课程大纲](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/outline.png) -[ORB-SLAM3 逐行源码讲解](https://appafc4omci9700.h5.xiaoeknow.com/content_page/eyJ0eXBlIjoiMyIsInJlc291cmNlX3R5cGUiOjI1LCJyZXNvdXJjZV9pZCI6IiIsImFwcF9pZCI6ImFwcGFmYzRvbWNpOTcwMCIsInByb2R1Y3RfaWQiOiJ0ZXJtXzYxMzcxNTEwODY4MzNfQXRXa2YzIiwiZXhwYW5kX2RhdGEiOltdfQ) +[ORB-SLAM3 逐行源码讲解](https://cvlife.net/detail/term_6255372036a53_o5VfgR/25) ---- # ORB-SLAM3 diff --git a/include/ImuTypes.h b/include/ImuTypes.h index ac78d36..79d9e4a 100644 --- a/include/ImuTypes.h +++ b/include/ImuTypes.h @@ -223,7 +223,7 @@ public: private: // Updated bias - Bias bu; + Bias bu; //更新后的零偏 // Dif between original and updated bias // This is used to compute the updated values of the preintegration Eigen::Matrix db; diff --git a/outline.png b/outline.png index eb25edb..4c016a5 100644 Binary files a/outline.png and b/outline.png differ diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc index 38bd286..34f39c0 100644 --- a/src/G2oTypes.cc +++ b/src/G2oTypes.cc @@ -579,12 +579,12 @@ EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast( void EdgeInertial::computeError() { // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big - const VertexPose* VP1 = static_cast(_vertices[0]); - const VertexVelocity* VV1= static_cast(_vertices[1]); - const VertexGyroBias* VG1= static_cast(_vertices[2]); - const VertexAccBias* VA1= static_cast(_vertices[3]); - const VertexPose* VP2 = static_cast(_vertices[4]); - const VertexVelocity* VV2 = static_cast(_vertices[5]); + const VertexPose* VP1 = static_cast(_vertices[0]); //位姿Ti + const VertexVelocity* VV1= static_cast(_vertices[1]); //速度vi + const VertexGyroBias* VG1= static_cast(_vertices[2]); //零偏Bgi + const VertexAccBias* VA1= static_cast(_vertices[3]); //零偏Bai + const VertexPose* VP2 = static_cast(_vertices[4]); //位姿Tj + const VertexVelocity* VV2 = static_cast(_vertices[5]); //速度vj const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]); const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast(); const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast(); diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc index 6dd0a93..0656edf 100644 --- a/src/ImuTypes.cc +++ b/src/ImuTypes.cc @@ -242,7 +242,7 @@ void Preintegrated::Reintegrate() * * @param[in] acceleration 加速度计数据 * @param[in] angVel 陀螺仪数据 - * @param[in] dt 两帧之间时间差 + * @param[in] dt 两图像 帧之间时间差 */ void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt) { diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 0fb31b7..a2e3455 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -1547,10 +1547,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) pKF = pKF->mPrevKF; } lpKF.push_front(pKF); - // 以相同内容再构建一个vector + // 同样内容再构建一个和lpKF一样的容器vpKF vector vpKF(lpKF.begin(),lpKF.end()); - - // TODO 跟上面重复？ if(vpKF.size()mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) && - mpCurrentKF->GetMap()->GetIniertialBA1()) + // 通过物理约束来保证两个坐标轴都是水平的 + if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) && + mpCurrentKF->GetMap()->GetIniertialBA1()) // TODO, maybe with GetIniertialBA1 { Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix()); phi(0)=0; @@ -465,6 +465,7 @@ bool LoopClosing::NewDetectCommonRegions() mnLoopNumCoincidences++; // 不再参与新的回环检测 mpLoopLastCurrentKF->SetErase(); + // 将当前关键帧作为上次关键帧 mpLoopLastCurrentKF = mpCurrentKF; mg2oLoopSlw = gScw; // 记录当前优化的结果为{last T_cw}即为 T_lw // 记录匹配到的点 @@ -505,7 +506,8 @@ bool LoopClosing::NewDetectCommonRegions() //Merge candidates bool bMergeDetectedInKF = false; // Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面 - // 如果成功验证总次数大于0 + // mnMergeNumCoincidences表示成功校验总次数，初始化为0 + // 会先经过后面共视几何校验，如果小于3，会进到如下判断开始时序几何校验 if(mnMergeNumCoincidences > 0) { // Find from the last KF candidates @@ -534,7 +536,7 @@ bool LoopClosing::NewDetectCommonRegions() mpMergeLastCurrentKF = mpCurrentKF; mg2oMergeSlw = gScw; mvpMergeMatchedMPs = vpMatchedMPs; - // 如果验证数大于等于3则为成功回环 + // 如果验证数大于等于3则为成功 mbMergeDetected = mnMergeNumCoincidences >= 3; } // 如果没找到共同区域(时序验证失败一次) @@ -690,8 +692,9 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* // 若匹配的数量大于一定的数目 if(numOptMatches > nProjOptMatches) { - //!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm.t - g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0); + //!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1 + g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)), + Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0); vector vpMatchedMP; vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); @@ -1548,8 +1551,8 @@ void LoopClosing::CorrectLoop() mpAtlas->InformNewBigChange(); // Add loop edge - // Step 8. 添加当前帧与闭环匹配帧之间的边（这个连接关系不优化） - // !感觉这两句话应该放在OptimizeEssentialGraph之前，因为OptimizeEssentialGraph的步骤4.2中有优化 + // Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化） + // 它在下一次的Essential Graph里面使用 mpLoopMatchedKF->AddLoopEdge(mpCurrentKF); mpCurrentKF->AddLoopEdge(mpLoopMatchedKF); @@ -2110,8 +2113,8 @@ void LoopClosing::MergeLocal() bool bStop = false; // 为Local BA的接口, 把set转为vector // Step 7 在缝合(Welding)区域进行local BA - vpLocalCurrentWindowKFs.clear(); - vpMergeConnectedKFs.clear(); + vpLocalCurrentWindowKFs.clear(); //当前关键帧的窗口 + vpMergeConnectedKFs.clear(); //融合关键帧的窗口 std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs)); std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs)); if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) @@ -2272,8 +2275,8 @@ void LoopClosing::MergeLocal() // Essential graph 优化后可以重新开始局部建图了 mpLocalMapper->Release(); - // 如果之前停掉了全局的BA,就开启全局BA - // 这里没有imu, 所以isImuInitialized一定是false, 所以第二个条件（当前地图关键帧数量小于200且地图只有一个）一定是true + // 全局的BA（永远不会执行） + // 这里没有imu, 所以isImuInitialized一定是false, 此时地图融合Atlas至少2个地图，所以第二个条件也一定是false // Step 9 全局BA if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1))) { @@ -2404,7 +2407,7 @@ void LoopClosing::MergeLocal2() 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 更新地图 + // 用优化得到的 bias 更新普通帧位姿 mpTracker->UpdateFrameIMU(1.0f,b,mpTracker->GetLastKeyFrame()); // Set map initialized @@ -2457,9 +2460,9 @@ void LoopClosing::MergeLocal2() pMergeMap->EraseMapPoint(pMPi); } // ? BUG! pMergeMap没有设置为BAD - // ? 感觉应该加入如下代码吧？ - // ? mpAtlas->SetMapBad(pCurrentMap); - + // ? 应该加入如下代码吧？ + // ? mpAtlas->SetMapBad(pMergeMap); + // Save non corrected poses (already merged maps) // 存下所有关键帧在融合矫正之前的位姿 vector vpKFs = pCurrentMap->GetAllKeyFrames(); @@ -2510,7 +2513,7 @@ void LoopClosing::MergeLocal2() vector vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge) vector vpCurrentConnectedKFs; - // 为后续SearchAndFuse及welding BA准备数据 + // 为后续SearchAndFuse准备数据 // 拿出融合帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧 mvpMergeConnectedKFs.push_back(mpMergeMatchedKF); vector aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames(); diff --git a/src/Optimizer.cc b/src/Optimizer.cc index b785be3..6e5d9ca 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -4551,8 +4551,10 @@ int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit cv::KeyPoint kpUn; // Left monocular observation + // 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1 if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft) { + //如果是两个相机情况下的相机1 if(i < Nleft) // pair left-right kpUn = pFrame->mvKeys[i]; else @@ -4934,8 +4936,10 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit) { cv::KeyPoint kpUn; // Left monocular observation + // 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1 if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft) { + //如果是两个相机情况下的相机1 if(i < Nleft) // pair left-right kpUn = pFrame->mvKeys[i]; else diff --git a/src/Tracking.cc b/src/Tracking.cc index 56b206b..36014ab 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -3515,11 +3515,13 @@ bool Tracking::TrackLocalMap() if(!mbMapUpdated) // && (mnMatchesInliers>30)) { Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG); + // 使用上一普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏 inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); } else { Verbose::PrintMess("TLM: PoseInertialOptimizationLastKeyFrame ", Verbose::VERBOSITY_DEBUG); + // 使用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏 inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); } }