添加IMU注释内容

CMakeLists.txt

@@ -4,16 +4,16 @@ project(ORB_SLAM3)
 IF(NOT CMAKE_BUILD_TYPE)
   SET(CMAKE_BUILD_TYPE Release)
 ENDIF()
-
+# set(CMAKE_BUILD_TYPE "Debug")
 MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
 
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall   -O3")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3")
-set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
+# set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
-set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
+# set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
 
-# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-deprecated -O3 -march=native ")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-deprecated -O3 -march=native ")
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-deprecated -O3 -march=native")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-deprecated -O3 -march=native")
 
 # Check C++11 or C++0x support
 include(CheckCXXCompilerFlag)

Examples/Monocular-Inertial/mono_inertial_tum_vi.cc

@@ -103,8 +103,10 @@ int main(int argc, char **argv)
 
         // Find first imu to be considered, supposing imu measurements start first
         // Step 3 默认IMU数据早于图像数据记录，找到和第一帧图像时间戳最接近的imu时间戳索引，记录在first_imu[seq]中
-        while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][0])
+        while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][0]){
             first_imu[seq]++;
+            cout << "first_imu[seq] = "  << first_imu[seq] << endl;
+        }
         // 因为上面退出while循环时IMU时间戳刚刚超过图像时间戳，所以这里需要再减一个索引    
         first_imu[seq]--; // first imu measurement to be considered
 
@@ -125,22 +127,26 @@ int main(int argc, char **argv)
     // Step 4 SLAM系统的初始化，包括读取配置文件、字典，创建跟踪、局部建图、闭环、显示线程
     ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true, 0, file_name);
 
+    //遍历所有数据
     int proccIm = 0;
     for (seq = 0; seq<num_seq; seq++)
     {
         // Main loop
         cv::Mat im;
+        //存放imu数据容器,包含该加速度,角速度,时间戳
         vector<ORB_SLAM3::IMU::Point> vImuMeas;
         proccIm = 0;
+        //直方图均衡化,直方图均衡化的思想就是这样的:
+        //假设我有灰度级255的图像，但是都是属于［100，110］的灰度，图像对比度就很低，我应该尽可能拉到整个［0，255］
         cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
         {
             // Read image from file
-            // Step 5 读取每一帧图像并转换为灰度图存储在im
+            // Step 5 读取每一帧图像并转换为灰度图存储在im,seq表示第几个数据集,ni表示这个数据集的第几个数据
             im = cv::imread(vstrImageFilenames[seq][ni],cv::IMREAD_GRAYSCALE);
 
             // clahe
-            // ?这里是什么作用？
+            //直方图均衡化
             clahe->apply(im,im);
 
 
@@ -163,7 +169,7 @@ int main(int argc, char **argv)
             {
                 // cout << "t_cam " << tframe << endl;
                 // Step 6 把上一图像帧和当前图像帧之间的imu信息存储在vImuMeas里
-                // 注意第一个图像帧没有对应的imu数据
+                // 注意第一个图像帧没有对应的imu数据 //?是否存在一帧,因为之前是从最接近图像第一帧的imu算起,可能无效
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
                     vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,

build.sh

@@ -27,5 +27,5 @@ echo "Configuring and building ORB_SLAM3 ..."
 
 mkdir build
 cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
+cmake .. -DCMAKE_BUILD_TYPE=Debug
-make -j
+make

include/Map.h

@@ -193,7 +193,7 @@ protected:
     bool mHasTumbnail;
     bool mbBad = false;
 
-    bool mbIsInertial;
+    bool mbIsInertial;             //标记是否有imu
     bool mbIMU_BA1;
     bool mbIMU_BA2;
 

include/System.h

@@ -99,6 +99,17 @@ public:
 public:
 
     // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
+    /**
+     * @brief Construct a new System object
+     * 
+     * @param[in] strVocFile               //词袋文件所在路径
+     * @param[in] strSettingsFile          //配置文件所在路径 
+     * @param[in] sensor                   //传感器类型 
+     * @param[in] bUseViewer               //指定是否使用可视化界面
+     * @param[in] initFr                   //initFr表示初始化帧的id,开始设置为0 
+     * @param[out] strSequence             //序列名,在跟踪线程和局部建图线程用得到 
+     * @param[in] strLoadingFile           //看起来作者貌似想加地图重载功能的一个参数
+     */
     System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const int initFr = 0, const string &strSequence = std::string(), const string &strLoadingFile = std::string());
 
     // Proccess the given stereo frame. Images must be synchronized and rectified.

src/Atlas.cc

@@ -244,6 +244,7 @@ bool Atlas::isInertial()
 void Atlas::SetInertialSensor()
 {
     unique_lock<mutex> lock(mMutexAtlas);
+    //接着又调用Map类的SetInertialSensor成员函数,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关
     mpCurrentMap->SetInertialSensor();
 }
 

src/ImuTypes.cc

@@ -149,30 +149,44 @@ cv::Mat InverseRightJacobianSO3(const cv::Mat &v)
     return InverseRightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
 }
 
-
+/**
+ * @brief                  计算旋转角度积分量
+ * 
+ * @param[in] angVel       陀螺仪数据
+ * @param[in] imuBias      陀螺仪偏置
+ * @param[in] time         两帧间的时间差
+ */
 IntegratedRotation::IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time):
     deltaT(time)
 {
+    //得到考虑偏置后的角度旋转
     const float x = (angVel.x-imuBias.bwx)*time;
     const float y = (angVel.y-imuBias.bwy)*time;
     const float z = (angVel.z-imuBias.bwz)*time;
 
     cv::Mat I = cv::Mat::eye(3,3,CV_32F);
 
+    //计算旋转矩阵的模值，后面用罗德里格公式计算旋转矩阵时会用到
     const float d2 = x*x+y*y+z*z;
     const float d = sqrt(d2);
 
+    //角度转成叉积的矩阵形式
     cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
                  z, 0, -x,
                  -y,  x, 0);
+    // eps = 1e-4 是一个小量，根据罗德里格斯公式求极限，后面的高阶小量忽略掉得到此式
     if(d<eps)
     {
+        //forster 经典预积分论文公式（4）
         deltaR = I + W;
+        //小量时，右扰动 Jr = I
         rightJ = cv::Mat::eye(3,3,CV_32F);
     }
     else
     {
+        //forster 经典预积分论文公式（3）
         deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;
+        //forster 经典预积分论文公式（8）
         rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
     }
 }
@@ -252,8 +266,16 @@ void Preintegrated::Reintegrate()
         IntegrateNewMeasurement(aux[i].a,aux[i].w,aux[i].t);
 }
 
+/**
+ * @brief 预积分计算，更新noise
+ * 
+ * @param[in] acceleration  加速度计数据
+ * @param[in] angVel        陀螺仪数据
+ * @param[in] dt            两帧之间时间差
+ */
 void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt)
 {
+    // 保存imu数据，利用中值积分的结果构造一个预积分类保存在mvMeasurements中
     mvMeasurements.push_back(integrable(acceleration,angVel,dt));
 
     // Position is updated firstly, as it depends on previously computed velocity and rotation.
@@ -261,20 +283,28 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con
     // Rotation is the last to be updated.
 
     //Matrices to compute covariance
+    // Step 1.构造协方差矩阵 参考Forster论文公式（62），邱笑晨的《预积分总结与公式推导》的P12页也有详细推导:η_ij = A * η_i,j-1 + B_j-1 * η_j-1
+    // ? 位姿第一个被更新，速度第二（因为这两个只依赖前一帧计算的旋转矩阵和速度），后面再更新旋转角度
+    // 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差
     cv::Mat A = cv::Mat::eye(9,9,CV_32F);
+    // 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用
     cv::Mat B = cv::Mat::zeros(9,6,CV_32F);
     
+    // 考虑偏置后的加速度、角速度
     cv::Mat acc = (cv::Mat_<float>(3,1) << acceleration.x-b.bax,acceleration.y-b.bay, acceleration.z-b.baz);
     cv::Mat accW = (cv::Mat_<float>(3,1) << angVel.x-b.bwx, angVel.y-b.bwy, angVel.z-b.bwz);
 
+    // 记录平均加速度和角速度
     avgA = (dT*avgA + dR*acc*dt)/(dT+dt);
     avgW = (dT*avgW + accW*dt)/(dT+dt);
     
     // Update delta position dP and velocity dV (rely on no-updated delta rotation)
+    // 根据没有更新的dR来更新dP与dV  eq.(38)
     dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
     dV = dV + dR*acc*dt;
 
     // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
+    // 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新
     cv::Mat Wacc = (cv::Mat_<float>(3,3) << 0, -acc.at<float>(2), acc.at<float>(1),
                                                    acc.at<float>(2), 0, -acc.at<float>(0),
                                                    -acc.at<float>(1), acc.at<float>(0), 0);
@@ -285,27 +315,36 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con
     B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt;
 
     // Update position and velocity jacobians wrt bias correction
+    // ? 更新bias雅克比,计算偏置的雅克比矩阵，pv 分别对ba与bg的偏导数,论文中没推这个值，邱笑晨那边有推导
+    // 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同
     JPa = JPa + JVa*dt -0.5f*dR*dt*dt;
     JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg;
     JVa = JVa - dR*dt;
     JVg = JVg - dR*dt*Wacc*JRg;
 
     // Update delta rotation
+    // Step 2. 构造函数，会根据更新后的bias进行角度积分
     IntegratedRotation dRi(angVel,b,dt);
+    // 强行归一化使其符合旋转矩阵的格式
     dR = NormalizeRotation(dR*dRi.deltaR);
 
     // Compute rotation parts of matrices A and B
+    // 补充AB矩阵
     A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t();
     B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt;
-
+    // 小量delta初始为0，更新后通常也为0，故省略了小量的更新
     // Update covariance
+    // Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响
     C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();
+    // 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵
     C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;
 
     // Update rotation jacobian wrt bias correction
+    // 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t
     JRg = dRi.deltaR.t()*JRg - dRi.rightJ*dt;
 
     // Total integrated time
+    // 更新总时间
     dT += dt;
 }
 

src/Map.cc

@@ -342,6 +342,7 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale
 void Map::SetInertialSensor()
 {
     unique_lock<mutex> lock(mMutexMap);
+    //将mbIsInertial设置为true,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关
     mbIsInertial = true;
 }
 

src/System.cc

@@ -37,11 +37,20 @@ namespace ORB_SLAM3
 {
 
 Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL;
-
+// 系统的构造函数，将会启动其他的线程
-System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
+System::System(const string &strVocFile,                //词袋文件所在路径
-               const bool bUseViewer, const int initFr, const string &strSequence, const string &strLoadingFile):
+               const string &strSettingsFile,           //配置文件所在路径 
-    mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),
+               const eSensor sensor,                    //传感器类型 
-    mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false)
+               const bool bUseViewer,                   //是否使用可视化界面 
+               const int initFr,                        //initFr表示初始化帧的id,开始设置为0
+               const string &strSequence,               //序列名,在跟踪线程和局部建图线程用得到
+               const string &strLoadingFile             //看起来作者貌似想加地图重载功能的一个参数
+               ):
+                mSensor(sensor),                        //初始化传感器类型
+                mpViewer(static_cast<Viewer*>(NULL)),   // ?空。。。对象指针？  TODO 
+                mbReset(false), mbResetActiveMap(false),// ?重新设置ActiveMap  
+                mbActivateLocalizationMode(false),      // ?是否开启局部定位功能开关
+                mbDeactivateLocalizationMode(false)     // ?没有这个模式转换标志
 {
     // Output welcome message
     cout << endl <<
@@ -54,15 +63,15 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
     cout << "Input sensor was set to: ";
     // Step 1 输出当前传感器类型
     if(mSensor==MONOCULAR)
-        cout << "Monocular" << endl;
+        cout << "Monocular" << endl;             //单目
     else if(mSensor==STEREO)
-        cout << "Stereo" << endl;
+        cout << "Stereo" << endl;                //双目
     else if(mSensor==RGBD)
-        cout << "RGB-D" << endl;
+        cout << "RGB-D" << endl;                 //RGBD相机   
     else if(mSensor==IMU_MONOCULAR)
-        cout << "Monocular-Inertial" << endl;
+        cout << "Monocular-Inertial" << endl;    //单目 + imu
     else if(mSensor==IMU_STEREO)
-        cout << "Stereo-Inertial" << endl;
+        cout << "Stereo-Inertial" << endl;       //双目 + imu
 
     //Check settings file
     // Step 2 读取配置文件
@@ -75,6 +84,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
        exit(-1);
     }
 
+    // ?ORBSLAM3新加的多地图管理功能，这里好像是加载Atlas标识符
     bool loadedAtlas = false;
 
     //----
@@ -172,6 +182,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
 
     // 设置Atlas中的传感器类型
     if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR)
+        // ? 如果是有imu的传感器类型，将mbIsInertial设置为imu属性,以后的跟踪和预积分将和这个标志有关
         mpAtlas->SetInertialSensor();
 
     // Step 6 依次创建跟踪、局部建图、闭环、显示线程
@@ -182,7 +193,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
 
     //Initialize the Tracking thread
     //(it will live in the main thread of execution, the one that called this constructor)
-    // 创建跟踪线程（主线程）
+    // 创建跟踪线程（主线程）,不会立刻开启,会在对图像和imu预处理后在main主线程种执行
     cout << "Seq. Name: " << strSequence << endl;
     mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                              mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, strSequence);
@@ -196,6 +207,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
     mpLocalMapper->mInitFr = initFr;
     //设置最远3D地图点的深度值，如果超过阈值，说明可能三角化不太准确，丢弃
     mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
+    // ? 这里有个疑问,C++中浮点型跟0比较是否用精确?
     if(mpLocalMapper->mThFarPoints!=0)
     {
         cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;
@@ -381,6 +393,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
 
     // Check mode change
     {
+        // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
         unique_lock<mutex> lock(mMutexMode);
         // mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式
         if(mbActivateLocalizationMode)
@@ -394,6 +407,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
             }
             // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
             mpTracker->InformOnlyTracking(true);
+            // 关闭线程可以使得别的线程得到更多的资源
             mbActivateLocalizationMode = false;
         }
         if(mbDeactivateLocalizationMode)
@@ -413,6 +427,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
             mbReset = false;
             mbResetActiveMap = false;
         }
+        //如果检测到重置活动地图,讲重置地图设置
         else if(mbResetActiveMap)
         {
             cout << "SYSTEM-> Reseting active map in monocular case" << endl;

src/Tracking.cc

@@ -1073,7 +1073,8 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp,
     }
     else if(mSensor == System::IMU_MONOCULAR)
     {
-        if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
+        //判断该帧是不是初始化
+        if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)  //没有成功初始化的前一个状态就是NO_IMAGES_YET
         {
             mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
         }
@@ -1104,6 +1105,7 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp,
     f_track_stats << mvpLocalMapPoints.size() << ",";
     f_track_stats << setprecision(6) << t_track << endl;*/
 
+//开启保存数据模式
 #ifdef SAVE_TIMES
     f_track_times << mCurrentFrame.mTimeORB_Ext << ",";
     f_track_times << mCurrentFrame.mTimeStereoMatch << ",";
@@ -1118,7 +1120,7 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp,
     return mCurrentFrame.mTcw.clone();
 }
 
-
+//将imu数据存放在mlQueueImuData的list链表里
 void Tracking::GrabImuData(const IMU::Point &imuMeasurement)
 {
     unique_lock<mutex> lock(mMutexImuQueue);
@@ -1127,11 +1129,13 @@ void Tracking::GrabImuData(const IMU::Point &imuMeasurement)
 
 void Tracking::PreintegrateIMU()
 {
+    // Step 1.拿到两两帧之间待处理的预积分数据，组成一个集合 
     // cout << "start preintegration" << endl;
-
+    // 上一帧不存在,说明两帧之间没有imu数据，不进行预积分
     if(!mCurrentFrame.mpPrevFrame)
     {
         Verbose::PrintMess("non prev frame ", Verbose::VERBOSITY_NORMAL);
+        // ? 当前帧是否进行预积分标志
         mCurrentFrame.setIntegrated();
         return;
     }
@@ -1140,6 +1144,7 @@ void Tracking::PreintegrateIMU()
 
     mvImuFromLastFrame.clear();
     mvImuFromLastFrame.reserve(mlQueueImuData.size());
+    // 没有imu数据,不进行预积分
     if(mlQueueImuData.size() == 0)
     {
         Verbose::PrintMess("Not IMU data in mlQueueImuData!!", Verbose::VERBOSITY_NORMAL);
@@ -1149,17 +1154,22 @@ void Tracking::PreintegrateIMU()
 
     while(true)
     {
+        // 数据还没有时,会等待一段时间,直到mlQueueImuData中有imu数据.一开始不需要等待
         bool bSleep = false;
         {
             unique_lock<mutex> lock(mMutexImuQueue);
             if(!mlQueueImuData.empty())
             {
+                // 拿到第一个imu数据作为起始数据
                 IMU::Point* m = &mlQueueImuData.front();
                 cout.precision(17);
+                // imu起始数据会比当前帧的前一帧时间戳早,如果相差0.001则舍弃这个imu数据
                 if(m->t<mCurrentFrame.mpPrevFrame->mTimeStamp-0.001l)
                 {
                     mlQueueImuData.pop_front();
                 }
+                // 同样最后一个的imu数据时间戳也不能理当前帧时间间隔多余0.001 
+                // ? (时间戳本身的差值,单位是s秒,tum数据集里面是19位时间戳,单位为ns,这里可能只保留了10位整数和9位小数)
                 else if(m->t<mCurrentFrame.mTimeStamp-0.001l)
                 {
                     mvImuFromLastFrame.push_back(*m);
@@ -1167,6 +1177,7 @@ void Tracking::PreintegrateIMU()
                 }
                 else
                 {
+                    // 得到两帧间的imu数据放入mvImuFromLastFrame中,得到后面预积分的处理数据
                     mvImuFromLastFrame.push_back(*m);
                     break;
                 }
@@ -1181,32 +1192,55 @@ void Tracking::PreintegrateIMU()
             usleep(500);
     }
 
-
+    // Step 2.对两帧之间进行中值积分处理
+    // m个imu组数据会有m-1个预积分量
     const int n = mvImuFromLastFrame.size()-1;
+    // 构造imu预处理器,并初始化标定数据
     IMU::Preintegrated* pImuPreintegratedFromLastFrame = new IMU::Preintegrated(mLastFrame.mImuBias,mCurrentFrame.mImuCalib);
-
+    // 针对预积分位置的不同做不同中值积分的处理
+    /**
+     *  根据上面imu帧的筛选，IMU与图像帧的时序如下：
+     *  Frame---IMU0---IMU1---IMU2---IMU3---IMU4---------------IMUx---Frame---IMUx+1
+     *  T_------T0-----T1-----T2-----T3-----T4-----------------Tx-----_T------Tx+1
+     *  A_------A0-----A1-----A2-----A3-----A4-----------------Ax-----_T------Ax+1
+     *  W_------W0-----W1-----W2-----W3-----W4-----------------Wx-----_T------Wx+1
+     *  T_和_T分别表示上一图像帧和当前图像帧的时间戳，A(加速度数据)，W(陀螺仪数据)，同理
+     */
     for(int i=0; i<n; i++)
     {
         float tstep;
         cv::Point3f acc, angVel;
+        // 第一帧数据但不是最后两帧,imu总帧数大于2
         if((i==0) && (i<(n-1)))
         {
+            // 时间差作为积分量
             float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;
+            // 根据上面分析的IMU与图像帧的时序分析，获取当前imu到上一帧的时间间隔
+            // 在加加速度不变的假设上算出第一帧图像到第二个imu数据的中值积分
             float tini = mvImuFromLastFrame[i].t-mCurrentFrame.mpPrevFrame->mTimeStamp;
+            // ? 这里采用离散中值积分进行预积分,获取当前imu到上一帧的时间间隔
+            // 设当前时刻imu的加速度a0，下一时刻加速度a1，时间间隔tab 为t10，tini t0p
+            // 正常情况下时为了求上一帧到当前时刻imu的一个平均加速度，但是imu时间不会正好落在上一帧图像的时刻，需要做补偿，要求得a0时刻到上一帧这段时间加速度的改变量
+            // 有了这个改变量将其加到a0上之后就可以表示上一帧时的加速度了。其中a0 - (a1-a0)*(tini/tab) 为上一帧时刻的加速度再加上a1 之后除以2就为这段时间的加速度平均值
+            // 其中tstep表示a1到上一帧的时间间隔，a0 - (a1-a0)*(tini/tab)这个式子中tini可以是正也可以是负表示时间上的先后，(a1-a0)也是一样，多种情况下这个式子依然成立
             acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a-
                     (mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a)*(tini/tab))*0.5f;
             angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w-
                     (mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tini/tab))*0.5f;
+            // 上一帧图像到下一帧imu（也就是第二个imu数据）的时间差
             tstep = mvImuFromLastFrame[i+1].t-mCurrentFrame.mpPrevFrame->mTimeStamp;
         }
+        //中间帧直接求中值
         else if(i<(n-1))
         {
             acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a)*0.5f;
             angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w)*0.5f;
             tstep = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;
         }
+        //最后一帧,但不是第一帧
         else if((i>0) && (i==(n-1)))
         {
+            // 直到倒数第二个imu时刻时，计算过程跟第一时刻类似，都需要考虑帧与imu时刻的关系
             float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;
             float tend = mvImuFromLastFrame[i+1].t-mCurrentFrame.mTimeStamp;
             acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a-
@@ -1215,23 +1249,28 @@ void Tracking::PreintegrateIMU()
                     (mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tend/tab))*0.5f;
             tstep = mCurrentFrame.mTimeStamp-mvImuFromLastFrame[i].t;
         }
+        //只有两帧的情况
         else if((i==0) && (i==(n-1)))
         {
+            // ? 就两个数据时使用第一个时刻的，之前size减一了，最后一帧imu好像没遍历，这时候应该还有一帧在后面，但这里直接没有算中值积分？
             acc = mvImuFromLastFrame[i].a;
             angVel = mvImuFromLastFrame[i].w;
             tstep = mCurrentFrame.mTimeStamp-mCurrentFrame.mpPrevFrame->mTimeStamp;
         }
-
+        // Step 3.依次进行预积分计算
+        // 应该是必存在的吧，一个是相对上一关键帧，一个是相对上一帧
         if (!mpImuPreintegratedFromLastKF)
             cout << "mpImuPreintegratedFromLastKF does not exist" << endl;
         mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep);
         pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);
     }
 
+    // 记录当前预积分的图像帧
     mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame;
     mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
     mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;
 
+    // ? 设置为已预积分状态
     mCurrentFrame.setIntegrated();
 
     Verbose::PrintMess("Preintegration is finished!! ", Verbose::VERBOSITY_DEBUG);
@@ -1399,7 +1438,11 @@ void Tracking::ResetFrameIMU()
 
 /**
  * @brief 跟踪过程，包括恒速模型跟踪、参考关键帧跟踪、局部地图跟踪
+ * track包含两部分：估计运动、跟踪局部地图
  * 
+ * Step 1：初始化
+ * Step 2：跟踪
+ * Step 3：记录位姿信息，用于轨迹复现
  */
 void Tracking::Track()
 {
@@ -1409,7 +1452,7 @@ void Tracking::Track()
     mTime_LocalMapTrack = 0;
     mTime_NewKF_Dec = 0;
 #endif
-
+    // ? 还不清楚这个标识符,跟绘图相关?
     if (bStepByStep)
     {
         while(!mbStep)
@@ -1445,12 +1488,14 @@ void Tracking::Track()
         {
             // 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了，重置地图后直接返回
             cout << "id last: " << mLastFrame.mnId << "    id curr: " << mCurrentFrame.mnId << endl;
+            //根据是否是imu模式,进行imu的补偿
             if(mpAtlas->isInertial())
             {
                 // 如果当前地图imu成功初始化
                 if(mpAtlas->isImuInitialized())
                 {
                     cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
+                    // ? BA2标志代表什么?,BA优化成功?
                     if(!pCurrentMap->GetIniertialBA2())
                     {
                         // 如果当前子图中imu没有经过BA2，重置active地图
@@ -1474,8 +1519,9 @@ void Tracking::Track()
         }
     }
 
-    // Step 3 IMU模式下设置IMU的Bias参数
+    // Step 3 IMU模式下设置IMU的Bias参数,还要保证上一帧存在非空
     if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && mpLastKeyFrame)
+        //认为bias在两帧间不变
         mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());
 
     if(mState==NO_IMAGES_YET)
@@ -1484,7 +1530,7 @@ void Tracking::Track()
     }
 
     mLastProcessedState=mState;
-    // Step 4 IMU模式下对IMU数据进行预积分
+    // Step 4 IMU模式下对IMU数据进行预积分 -> // ? 没有创建地图的情况下
     if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && !mbCreatedMap)
     {
 #ifdef SAVE_TIMES