添加跟踪时IMU数据预测位姿的注释

src/Frame.cc

@@ -488,15 +488,24 @@ void Frame::SetVelocity(const cv::Mat &Vwb)
 {
     mVw = Vwb.clone();
 }
-
+/**
+ * @brief 设置IMU的速度预测位姿，最后用imu到相机的位姿和世界坐标系到imu的位姿更新相机和世界坐标系的位姿
+ *        目的：得到相机在世界坐标系的位姿
+ * 
+ * @param[in] Rwb     旋转值
+ * @param[in] twb     位移值  
+ * @param[in] Vwb     速度值
+ */
 void Frame::SetImuPoseVelocity(const cv::Mat &Rwb, const cv::Mat &twb, const cv::Mat &Vwb)
 {
+    //Twb -> Tbw 求逆的过程，根据旋转矩阵的逆求得，参见十四讲（第一版）P44公式（3.13）
     mVw = Vwb.clone();
     cv::Mat Rbw = Rwb.t();
     cv::Mat tbw = -Rbw*twb;
     cv::Mat Tbw = cv::Mat::eye(4,4,CV_32F);
     Rbw.copyTo(Tbw.rowRange(0,3).colRange(0,3));
     tbw.copyTo(Tbw.rowRange(0,3).col(3));
+    // 外参值mImuCalib.Tcb与Tbw求mTcw
     mTcw = mImuCalib.Tcb*Tbw;
     UpdatePoseMatrices();
 }

src/ImuTypes.cc

@@ -343,6 +343,7 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con
     // Update rotation jacobian wrt bias correction
     // 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t
     // 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212
+    // ? 为什么先更新JPa、JPg、JVa、JVg最后更新JRg? 答：这里必须先更新dRi才能更新到这个值，但是为什么JPg和JVg依赖的上一个JRg值进行更新的？
     JRg = dRi.deltaR.t()*JRg - dRi.rightJ*dt;
 
     // Total integrated time
@@ -398,7 +399,10 @@ IMU::Bias Preintegrated::GetDeltaBias(const Bias &b_)
 cv::Mat Preintegrated::GetDeltaRotation(const Bias &b_)
 {
     std::unique_lock<std::mutex> lock(mMutex);
+    // ? 计算偏置的变化量
     cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
+    // ? 考虑偏置后，dR对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13～P14
+    // ? Forster论文公式（44）yP17也有结果（但没有推导），后面两个函数GetDeltaPosition和GetDeltaPosition也是基于此推导的
     return NormalizeRotation(dR*ExpSO3(JRg*dbg));
 }
 
@@ -407,6 +411,7 @@ cv::Mat Preintegrated::GetDeltaVelocity(const Bias &b_)
     std::unique_lock<std::mutex> lock(mMutex);
     cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
     cv::Mat dba = (cv::Mat_<float>(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz);
+    // ? 考虑偏置后，dV对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 
     return dV + JVg*dbg + JVa*dba;
 }
 
@@ -415,6 +420,7 @@ cv::Mat Preintegrated::GetDeltaPosition(const Bias &b_)
     std::unique_lock<std::mutex> lock(mMutex);
     cv::Mat dbg = (cv::Mat_<float>(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
     cv::Mat dba = (cv::Mat_<float>(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz);
+    // ? 考虑偏置后，dP对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新
     return dP + JPg*dbg + JPa*dba;
 }
 

src/Tracking.cc

@@ -1276,7 +1276,12 @@ void Tracking::PreintegrateIMU()
     Verbose::PrintMess("Preintegration is finished!! ", Verbose::VERBOSITY_DEBUG);
 }
 
-
+/**
+ * @brief 跟踪不成功的时候，用初始化好的imu数据做跟踪处理
+ * 
+ * @return true 
+ * @return false 
+ */
 bool Tracking::PredictStateIMU()
 {
     if(!mCurrentFrame.mpPrevFrame)
@@ -1284,20 +1289,26 @@ bool Tracking::PredictStateIMU()
         Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL);
         return false;
     }
-
+    // 地图更新时，并且上一个图像关键帧存在
     if(mbMapUpdated && mpLastKeyFrame)
-    {
+    {   
         const cv::Mat twb1 = mpLastKeyFrame->GetImuPosition();
         const cv::Mat Rwb1 = mpLastKeyFrame->GetImuRotation();
         const cv::Mat Vwb1 = mpLastKeyFrame->GetVelocity();
 
         const cv::Mat Gz = (cv::Mat_<float>(3,1) << 0,0,-IMU::GRAVITY_VALUE);
+        // ? mpImuPreintegratedFromLastKF是将图像帧转成imu格式的？
         const float t12 = mpImuPreintegratedFromLastKF->dT;
-
+        // 计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿（预积分的值不会变化）与上一帧的位姿（会迭代变化）进行更新 
+        // 旋转 R_wb2 = R_wb1 * R_b1b2
         cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mpImuPreintegratedFromLastKF->GetDeltaRotation(mpLastKeyFrame->GetImuBias()));
+        // 位移 
         cv::Mat twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mpImuPreintegratedFromLastKF->GetDeltaPosition(mpLastKeyFrame->GetImuBias());
+        // 速度
         cv::Mat Vwb2 = Vwb1 + t12*Gz + Rwb1*mpImuPreintegratedFromLastKF->GetDeltaVelocity(mpLastKeyFrame->GetImuBias());
+        // 设置当前帧的世界坐标系的相机位姿
         mCurrentFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2);
+        // ? 预测当前帧imu在世界坐标系的位姿，以及记录偏置bias和bias更新
         mCurrentFrame.mPredRwb = Rwb2.clone();
         mCurrentFrame.mPredtwb = twb2.clone();
         mCurrentFrame.mPredVwb = Vwb2.clone();
@@ -1305,12 +1316,14 @@ bool Tracking::PredictStateIMU()
         mCurrentFrame.mPredBias = mCurrentFrame.mImuBias;
         return true;
     }
+    // 地图未更新时
     else if(!mbMapUpdated)
     {
         const cv::Mat twb1 = mLastFrame.GetImuPosition();
         const cv::Mat Rwb1 = mLastFrame.GetImuRotation();
         const cv::Mat Vwb1 = mLastFrame.mVw;
         const cv::Mat Gz = (cv::Mat_<float>(3,1) << 0,0,-IMU::GRAVITY_VALUE);
+        // ? mpImuPreintegratedFrame是当前帧上一帧，未必是关键帧的图像帧？
         const float t12 = mCurrentFrame.mpImuPreintegratedFrame->dT;
 
         cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mCurrentFrame.mpImuPreintegratedFrame->GetDeltaRotation(mLastFrame.mImuBias));