Merge branch 'master' of https://github.com/electech6/ORB_SLAM3_detailed_comments

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

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<float,6,1> db;

src/G2oTypes.cc

@@ -579,12 +579,12 @@ EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast<double>(
 void EdgeInertial::computeError()
 {
     // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
-    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);           //位姿Ti
-    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
+    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);    //速度vi
-    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
+    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);    //零偏Bgi
-    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
+    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);      //零偏Bai
-    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
+    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);           //位姿Tj
-    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
+    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_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<double>();
     const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast<double>();

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)
 {

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<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
-
-    // TODO 跟上面重复？
     if(vpKF.size()<nMinKF)
         return;
 

src/LoopClosing.cc

@@ -178,9 +178,9 @@ void LoopClosing::Run()
                             }
                             // If inertial, force only yaw
                             // 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0
-                            // 可以理解成两个坐标轴都经过了imu初始化，肯定都是水平的
+                            // 通过物理约束来保证两个坐标轴都是水平的
-                            if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&
+                            if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) &&
-                                   mpCurrentKF->GetMap()->GetIniertialBA1())
+                                   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
+            //!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1
-            g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0);
+            g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
+                           Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);
 
             vector<MapPoint*> vpMatchedMP;
             vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
@@ -1548,8 +1551,8 @@ void LoopClosing::CorrectLoop()
     mpAtlas->InformNewBigChange();
 
     // Add loop edge
-    // Step 8. 添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
+    // Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
-    // !感觉这两句话应该放在OptimizeEssentialGraph之前，因为OptimizeEssentialGraph的步骤4.2中有优化
+    // 它在下一次的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();
+    vpLocalCurrentWindowKFs.clear();    //当前关键帧的窗口
-    vpMergeConnectedKFs.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
+    // 全局的BA（永远不会执行）
-    // 这里没有imu, 所以isImuInitialized一定是false, 所以第二个条件（当前地图关键帧数量小于200且地图只有一个）一定是true 
+    // 这里没有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<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
-        // 用优化得到的 bias 更新地图
+        // 用优化得到的 bias 更新普通帧位姿
         mpTracker->UpdateFrameIMU(1.0f,b,mpTracker->GetLastKeyFrame());
 
         // Set map initialized
@@ -2457,8 +2460,8 @@ void LoopClosing::MergeLocal2()
             pMergeMap->EraseMapPoint(pMPi);
         }
         // ? BUG! pMergeMap没有设置为BAD
-        // ? 感觉应该加入如下代码吧？
+        // ? 应该加入如下代码吧？
-        // ? mpAtlas->SetMapBad(pCurrentMap);
+        // ? mpAtlas->SetMapBad(pMergeMap);
         
         // Save non corrected poses (already merged maps)
         // 存下所有关键帧在融合矫正之前的位姿
@@ -2510,7 +2513,7 @@ void LoopClosing::MergeLocal2()
     vector<MapPoint*> vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge)
     vector<KeyFrame*> vpCurrentConnectedKFs;
 
-    // 为后续SearchAndFuse及welding BA准备数据
+    // 为后续SearchAndFuse准备数据
     // 拿出融合帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧
     mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);
     vector<KeyFrame*> aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();

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

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());
             }
         }