更新标注

include/G2oTypes.h

@@ -67,7 +67,7 @@ Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const do
 
 Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R);
 
-
+// 相关节点中使用，存放的是imu与cam的内外参
 class ImuCamPose
 {
 public:
@@ -103,9 +103,10 @@ public:
     Eigen::Matrix3d Rwb0;
     Eigen::Matrix3d DR;
 
-    int its;
+    int its;  // 记录更新次数
 };
 
+// 逆深度点，后面节点虽然有用到，但是声明的节点并没有使用到，暂时不看
 class InvDepthPoint
 {
 public:
@@ -124,6 +125,7 @@ public:
 };
 
 // Optimizable parameters are IMU pose
+// 优化中关于位姿的节点，6自由度
 class VertexPose : public g2o::BaseVertex<6,ImuCamPose>
 {
 public:
@@ -140,15 +142,21 @@ public:
     virtual bool read(std::istream& is);
     virtual bool write(std::ostream& os) const;
 
+    // 重置函数,设定被优化变量的原始值
     virtual void setToOriginImpl() {
         }
 
+    // 更新
     virtual void oplusImpl(const double* update_){
         _estimate.Update(update_);
+        // https://github.com/RainerKuemmerle/g2o/blob/master/doc/README_IF_IT_WAS_WORKING_AND_IT_DOES_NOT.txt
+        // 官方讲解cache
+        // 需要在oplusImpl与setEstimate函数中添加
         updateCache();
     }
 };
 
+// 优化中关于位姿的节点，4自由度  3个平移加一个航偏
 class VertexPose4DoF : public g2o::BaseVertex<4,ImuCamPose>
 {
     // Translation and yaw are the only optimizable variables
@@ -172,6 +180,7 @@ public:
     virtual void setToOriginImpl() {
         }
 
+    // 强制让旋转的前两维的更新量为0
     virtual void oplusImpl(const double* update_){
         double update6DoF[6];
         update6DoF[0] = 0;
@@ -185,6 +194,9 @@ public:
     }
 };
 
+/** 
+ * @brief 速度节点
+ */
 class VertexVelocity : public g2o::BaseVertex<3,Eigen::Vector3d>
 {
 public:
@@ -206,6 +218,9 @@ public:
     }
 };
 
+/** 
+ * @brief 陀螺仪偏置节点
+ */
 class VertexGyroBias : public g2o::BaseVertex<3,Eigen::Vector3d>
 {
 public:
@@ -227,7 +242,9 @@ public:
     }
 };
 
-
+/** 
+ * @brief 加速度计偏置节点
+ */
 class VertexAccBias : public g2o::BaseVertex<3,Eigen::Vector3d>
 {
 public:
@@ -251,6 +268,7 @@ public:
 
 
 // Gravity direction vertex
+// 重力方向
 class GDirection
 {
 public:
@@ -268,6 +286,9 @@ public:
     int its;
 };
 
+/** 
+ * @brief 重力方向节点
+ */
 class VertexGDir : public g2o::BaseVertex<2,GDirection>
 {
 public:
@@ -290,6 +311,9 @@ public:
 };
 
 // scale vertex
+/** 
+ * @brief 尺度节点
+ */
 class VertexScale : public g2o::BaseVertex<1,double>
 {
 public:
@@ -315,6 +339,9 @@ public:
 
 
 // Inverse depth point (just one parameter, inverse depth at the host frame)
+/** 
+ * @brief 没用
+ */
 class VertexInvDepth : public g2o::BaseVertex<1,InvDepthPoint>
 {
 public:
@@ -337,12 +364,13 @@ public:
 };
 
 /** 
- * @brief 视觉重投影的边
+ * @brief 单目视觉重投影的边
  * 这里或许会有疑问，OptimizableTypes.h 里面不是定义了视觉重投影的边么？
  * 原因是这样，那里面定义的边也用，不过只是在纯视觉时，没有imu情况下，因为用已经定义好的节点就好
  * 但是加入imu时，优化要有残差的边与重投影的边同时存在，且两个边可能连接同一个位姿节点，所以需要重新弄一个包含imu位姿的节点
  * 因此，边也需要重新写，并且在imu优化时使用这个边
  */
+// 误差为2维， 类型为Eigen::Vector2d， 节点1类型为g2o::VertexSBAPointXYZ，节点二类型为VertexPose
 class EdgeMono : public g2o::BaseBinaryEdge<2,Eigen::Vector2d,g2o::VertexSBAPointXYZ,VertexPose>
 {
 public:
@@ -354,11 +382,12 @@ public:
     virtual bool read(std::istream& is){return false;}
     virtual bool write(std::ostream& os) const{return false;}
 
+    // 计算重投影误差
     void computeError(){
         const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
         const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
         const Eigen::Vector2d obs(_measurement);
-        _error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx);
+        _error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx);  // 投影
     }
 
 
@@ -379,6 +408,7 @@ public:
         return J;
     }
 
+    // 由2*2的像素点信息矩阵变成了9*9的关于旋转平移与三维点坐标的信息矩阵
     Eigen::Matrix<double,9,9> GetHessian(){
         linearizeOplus();
         Eigen::Matrix<double,2,9> J;
@@ -391,6 +421,9 @@ public:
     const int cam_idx;
 };
 
+/** 
+ * @brief 单目纯位姿一元边
+ */
 class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>
 {
 public:
@@ -426,11 +459,15 @@ public:
     const int cam_idx;
 };
 
+/** 
+ * @brief 双目位姿三维点二元边
+ */
 class EdgeStereo : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,g2o::VertexSBAPointXYZ,VertexPose>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+    // 代码中都是0
     EdgeStereo(int cam_idx_=0): cam_idx(cam_idx_){}
 
     virtual bool read(std::istream& is){return false;}
@@ -466,7 +503,9 @@ public:
     const int cam_idx;
 };
 
-
+/** 
+ * @brief 双目纯位姿一元边
+ */
 class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexPose>
 {
 public:
@@ -497,6 +536,9 @@ public:
 };
 
 // TO COMMENT
+/** 
+ * @brief 惯性边（误差为残差）
+ */
 class EdgeInertial : public g2o::BaseMultiEdge<9,Vector9d>
 {
 public:
@@ -510,6 +552,7 @@ public:
     void computeError();
     virtual void linearizeOplus();
 
+    // 关于pose1与2 的旋转平移速度，以及之间的偏置的信息矩阵
     Eigen::Matrix<double,24,24> GetHessian(){
         linearizeOplus();
         Eigen::Matrix<double,9,24> J;
@@ -522,6 +565,7 @@ public:
         return J.transpose()*information()*J;
     }
 
+    // 没用
     Eigen::Matrix<double,18,18> GetHessianNoPose1(){
         linearizeOplus();
         Eigen::Matrix<double,9,18> J;
@@ -533,6 +577,7 @@ public:
         return J.transpose()*information()*J;
     }
 
+    // 关于pose2 的旋转平移信息矩阵
     Eigen::Matrix<double,9,9> GetHessian2(){
         linearizeOplus();
         Eigen::Matrix<double,9,9> J;
@@ -541,14 +586,18 @@ public:
         return J.transpose()*information()*J;
     }
 
+    // 预积分中对应的状态对偏置的雅可比
     const Eigen::Matrix3d JRg, JVg, JPg;
     const Eigen::Matrix3d JVa, JPa;
-    IMU::Preintegrated* mpInt;
+
-    const double dt;
+    IMU::Preintegrated* mpInt;  // 预积分
-    Eigen::Vector3d g;
+    const double dt;  // 预积分时间
+    Eigen::Vector3d g;  // 0, 0, -IMU::GRAVITY_VALUE
 };
 
-
+/** 
+ * @brief 初始化惯性边（误差为残差）
+ */
 // Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale
 class EdgeInertialGS : public g2o::BaseMultiEdge<9,Vector9d>
 {
@@ -570,6 +619,7 @@ public:
     const double dt;
     Eigen::Vector3d g, gI;
 
+    // 关于pose1与2 的旋转平移速度，以及之间的偏置，还有重力方向与尺度的信息矩阵
     Eigen::Matrix<double,27,27> GetHessian(){
         linearizeOplus();
         Eigen::Matrix<double,9,27> J;
@@ -584,6 +634,7 @@ public:
         return J.transpose()*information()*J;
     }
 
+    // 与上面摆放不同
     Eigen::Matrix<double,27,27> GetHessian2(){
         linearizeOplus();
         Eigen::Matrix<double,9,27> J;
@@ -598,6 +649,7 @@ public:
         return J.transpose()*information()*J;
     }
 
+    // 关于偏置，重力方向与尺度的信息矩阵
     Eigen::Matrix<double,9,9> GetHessian3(){
         linearizeOplus();
         Eigen::Matrix<double,9,9> J;
@@ -609,7 +661,7 @@ public:
     }
 
 
-
+    // 下面的没有用到，其实也是获取状态的信息矩阵
     Eigen::Matrix<double,1,1> GetHessianScale(){
         linearizeOplus();
         Eigen::Matrix<double,9,1> J = _jacobianOplus[7];
@@ -636,7 +688,9 @@ public:
 };
 
 
-
+/** 
+ * @brief 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
+ */
 class EdgeGyroRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexGyroBias,VertexGyroBias>
 {
 public:
@@ -672,7 +726,9 @@ public:
     }
 };
 
-
+/** 
+ * @brief 加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
+ */
 class EdgeAccRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexAccBias,VertexAccBias>
 {
 public:
@@ -708,6 +764,9 @@ public:
     }
 };
 
+/** 
+ * @brief 先验类
+ */
 class ConstraintPoseImu
 {
 public:
@@ -717,7 +776,8 @@ public:
                        const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_):
                        Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_)
     {
-        H = (H+H)/2;
+        H = (H+H)/2;  // 对称化
+        // 令特征值小的位置变为0
         Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,15,15> > es(H);
         Eigen::Matrix<double,15,1> eigs = es.eigenvalues();
         for(int i=0;i<15;i++)
@@ -725,6 +785,8 @@ public:
                 eigs[i]=0;
         H = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
     }
+
+    // 形式不同
     ConstraintPoseImu(const cv::Mat &Rwb_, const cv::Mat &twb_, const cv::Mat &vwb_,
                        const IMU::Bias &b, const cv::Mat &H_)
     {
@@ -753,6 +815,9 @@ public:
     Matrix15d H;
 };
 
+/** 
+ * @brief 先验边，前端优化单帧用到
+ */
 class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d>
 {
 public:
@@ -775,6 +840,7 @@ public:
             return J.transpose()*information()*J;
         }
 
+        // 没用到，除了旋转平移外的信息矩阵
         Eigen::Matrix<double,9,9> GetHessianNoPose(){
             linearizeOplus();
             Eigen::Matrix<double,15,9> J;
@@ -789,6 +855,9 @@ public:
 };
 
 // Priors for biases
+/** 
+ * @brief 根据给定值的加速度计先验边，目的是把优化量维持在先验附近
+ */
 class EdgePriorAcc : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexAccBias>
 {
 public:
@@ -813,6 +882,9 @@ public:
     const Eigen::Vector3d bprior;
 };
 
+/** 
+ * @brief 根据给定值的陀螺仪先验边，目的是把优化量维持在先验附近
+ */
 class EdgePriorGyro : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexGyroBias>
 {
 public:
@@ -837,7 +909,9 @@ public:
     const Eigen::Vector3d bprior;
 };
 
-
+/** 
+ * @brief 4DOF的二元边，误差为给定的旋转平移改变量 与 两个输入节点之间的旋转平移改变量的偏差
+ */
 class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose4DoF>
 {
 public:

src/G2oTypes.cc

@@ -167,6 +167,9 @@ void ImuCamPose::SetParam(const std::vector<Eigen::Matrix3d> &_Rcw, const std::v
     bf = _bf;
 }
 
+/** 
+ * @brief 单目投影
+ */
 Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) const
 {
     Eigen::Vector3d Xc = Rcw[cam_idx]*Xw+tcw[cam_idx];
@@ -174,6 +177,10 @@ Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) cons
     return pCamera[cam_idx]->project(Xc);
 }
 
+/** 
+ * @brief 双目投影，都是0
+ * @return u v u`
+ */
 Eigen::Vector3d ImuCamPose::ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx) const
 {
     Eigen::Vector3d Pc = Rcw[cam_idx]*Xw+tcw[cam_idx];
@@ -219,6 +226,7 @@ void ImuCamPose::Update(const double *pu)
 
 }
 
+// 更新世界坐标系
 void ImuCamPose::UpdateW(const double *pu)
 {
     Eigen::Vector3d ur, ut;
@@ -606,7 +614,7 @@ void EdgeInertial::linearizeOplus()
     _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK
 
     // Jacobians wrt Pose 2
-    // _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
+    // _jacobianOplus[4] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
     _jacobianOplus[4].setZero();
     // rotation
     _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK
@@ -614,7 +622,7 @@ void EdgeInertial::linearizeOplus()
     _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK
 
     // Jacobians wrt Velocity 2
-    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
+    // _jacobianOplus[5] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
     _jacobianOplus[5].setZero();
     _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
 }
@@ -930,6 +938,6 @@ Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R)
     // 2. 对于行列数不同的矩阵，例如3*4 或者 4*3 矩阵只有3个奇异向量，计算的时候如果是Thin 那么得出的UV矩阵列数只能是3，如果是full那么就是4
     // 3. thin会损失一部分数据，但是会加快计算，对于大型矩阵解算方程时，可以用thin加速得到结果
     Eigen::JacobiSVD<Eigen::Matrix3d> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV);
-    return svd.matrixU()*svd.matrixV();
+    return svd.matrixU()*svd.matrixV().transpose();
 }
 }

src/LocalMapping.cc

@@ -1600,7 +1600,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     mFirstTs=vpKF.front()->mTimeStamp;
     if(mpCurrentKeyFrame->mTimeStamp-mFirstTs<minTime)
         return;
-
+    
+    // 正在做IMU的初始化，在tracking里面使用，如果为true，暂不添加关键帧
     bInitializing = true;
 
     // 先处理新关键帧，防止堆积且保证数据量充足
@@ -1626,20 +1627,21 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
                 continue;
             if (!(*itKF)->mPrevKF)
                 continue;
-            // Rwb（imu坐标转到初始化前世界坐标系下的坐标）*更新偏置后的速度，可以理解为在世界坐标系下的速度矢量
+            // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij)
             dirG -= (*itKF)->mPrevKF->GetImuRotation()*(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
             // 求取实际的速度，位移/时间
             cv::Mat _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT;
             (*itKF)->SetVelocity(_vel);
             (*itKF)->mPrevKF->SetVelocity(_vel);
         }
+        // dirG = sV1 - sVn + n*Rwg*g*t
         // 归一化
         dirG = dirG/cv::norm(dirG);
         // 原本的重力方向
         cv::Mat gI = (cv::Mat_<float>(3,1) << 0.0f, 0.0f, -1.0f);
         // 求速度方向与重力方向的角轴
         cv::Mat v = gI.cross(dirG);
-        // 求角轴长度
+        // 求角轴模长
         const float nv = cv::norm(v);
         // 求转角大小
         const float cosg = gI.dot(dirG);
@@ -1664,7 +1666,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp;
 
     std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
-    // 计算残差及偏置差，优化尺度重力方向及Ri Rj Vi Vj Pi Pj
+    // 计算残差及偏置差，优化尺度重力方向及速度偏置，偏置先验为0，双目时不优化尺度
     Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
@@ -1736,10 +1738,11 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     }
 
 
-    mbNewInit=true;
+    mbNewInit=true;  // 没用到
-    mnKFs=vpKF.size();
+    mnKFs=vpKF.size();  // 没用到
-    mIdxInit++;
+    mIdxInit++;  // 没用到
 
+    // 里面存放的是来到Local线程，但还未处理的关键帧
     for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
     {
         (*lit)->SetBadFlag();

src/Map.cc

@@ -298,6 +298,14 @@ void Map::RotateMap(const cv::Mat &R)
     }
 }
 
+// 恢复尺度及重力方向
+/** imu在localmapping中初始化，LocalMapping::InitializeIMU中使用，误差包含三个残差与两个偏置
+ * 地图融合时也会使用
+ * @param R 初始化时为Rgw
+ * @param s 尺度
+ * @param bScaledVel 将尺度更新到速度
+ * @param t 默认cv::Mat::zeros(cv::Size(1,3),CV_32F)
+ */
 void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScaledVel, const cv::Mat t)
 {
     unique_lock<mutex> lock(mMutexMap);
@@ -310,19 +318,36 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale
 
     cv::Mat Tyw = Tyx*Txw;
     Tyw.rowRange(0,3).col(3) = Tyw.rowRange(0,3).col(3)+t;
-    cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3);
+    // Tyw 中旋转部分等于R，平移部分等于t
-    cv::Mat tyw = Tyw.rowRange(0,3).col(3);
+    // 做了很多操作，到最后还是得出了一个R跟t？感觉上面的操作像是预留了一些东西，比如在世界坐标系与第一帧有一定Rt时，只不过暂时没有用到
+    cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3);  // R
+    cv::Mat tyw = Tyw.rowRange(0,3).col(3);         // t
 
     for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(); sit!=mspKeyFrames.end(); sit++)
     {
+        // 更新关键帧位姿
+        /**
+         * | Rw2w1  tw2w1 |   *   | Rw1c  s*tw1c  |     =    |  Rw2c     s*Rw2w1*tw1c + tw2w1  |
+         * |   0      1   |       |  0       1    |          |   0                1            |
+         * 这么做比正常乘在旋转上少了个s，后面不需要这个s了，因为所有mp在下面已经全部转到了w2坐标系下，不存在尺度变化了
+         * 
+         * | s*Rw2w1  tw2w1 |   *   | Rw1c    tw1c  |     =    |  s*Rw2c     s*Rw2w1*tw1c + tw2w1  |
+         * |   0        1   |       |  0       1    |          |     0                1            |
+         */
+
         KeyFrame* pKF = *sit;
         cv::Mat Twc = pKF->GetPoseInverse();
         Twc.rowRange(0,3).col(3)*=s;
+
+        // |  Ryc     s*Ryw*twc + tyw  |
+        // |   0           1           |
         cv::Mat Tyc = Tyw*Twc;
+
         cv::Mat Tcy = cv::Mat::eye(4,4,CV_32F);
         Tcy.rowRange(0,3).colRange(0,3) = Tyc.rowRange(0,3).colRange(0,3).t();
         Tcy.rowRange(0,3).col(3) = -Tcy.rowRange(0,3).colRange(0,3)*Tyc.rowRange(0,3).col(3);
         pKF->SetPose(Tcy);
+        // 更新关键帧速度
         cv::Mat Vw = pKF->GetVelocity();
         if(!bScaledVel)
             pKF->SetVelocity(Ryw*Vw);
@@ -332,6 +357,7 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale
     }
     for(set<MapPoint*>::iterator sit=mspMapPoints.begin(); sit!=mspMapPoints.end(); sit++)
     {
+        // 更新每一个mp在世界坐标系下的坐标
         MapPoint* pMP = *sit;
         pMP->SetWorldPos(s*Ryw*pMP->GetWorldPos()+tyw);
         pMP->UpdateNormalAndDepth();

src/Optimizer.cc

@@ -5069,7 +5069,8 @@ Eigen::MatrixXd Optimizer::Sparsify(const Eigen::MatrixXd &H, const int &start1,
 }
 
 
-void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd  &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA)
+void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba,
+                                    bool bMono, Eigen::MatrixXd  &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA)
 {
     Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL);
     int its = 200; // Check number of iterations

src/Tracking.cc

@@ -4582,12 +4582,25 @@ void Tracking::InformOnlyTracking(const bool &flag)
     mbOnlyTracking = flag;
 }
 
+/**
+ * @brief 更新了关键帧的位姿，但需要修改普通帧的位姿，因为正常跟踪需要普通帧
+ * localmapping中初始化imu中使用，速度的走向（仅在imu模式使用），最开始速度定义于imu初始化时，每个关键帧都根据位移除以时间得到，经过非线性优化保存于KF中.
+ * 之后使用本函数，让上一帧与当前帧分别与他们对应的上一关键帧做速度叠加得到，后面新的frame速度由上一个帧速度决定，如果使用匀速模型（大多数情况下），通过imu积分更新速度。
+ * 新的关键帧继承于对应帧
+ * @param  s 尺度
+ * @param  b 初始化后第一帧的偏置
+ * @param  pCurrentKeyFrame 当前关键帧
+ */
 void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame)
 {
     Map * pMap = pCurrentKeyFrame->GetMap();
-    unsigned int index = mnFirstFrameId;
+    unsigned int index = mnFirstFrameId;  // unused
+
+    // 每一帧的参考关键帧
     list<ORB_SLAM3::KeyFrame*>::iterator lRit = mlpReferences.begin();
-    list<bool>::iterator lbL = mlbLost.begin();
+    list<bool>::iterator lbL = mlbLost.begin();  // 对应帧是否跟踪丢失
+    // mlRelativeFramePoses 存放的是Tcr
+    // 三个变量一一对应
     for(list<cv::Mat>::iterator lit=mlRelativeFramePoses.begin(),lend=mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lbL++)
     {
         if(*lbL)
@@ -4605,11 +4618,11 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr
             (*lit).rowRange(0,3).col(3)=(*lit).rowRange(0,3).col(3)*s;
         }
     }
-
+    // 设置偏置
     mLastBias = b;
-
+    // 设置上一关键帧，如果说mpLastKeyFrame已经是经过添加的新的kf，而pCurrentKeyFrame还是上一个kf，mpLastKeyFrame直接指向之前的kf
     mpLastKeyFrame = pCurrentKeyFrame;
-
+    // 更新偏置
     mLastFrame.SetNewBias(mLastBias);
     mCurrentFrame.SetNewBias(mLastBias);
 
@@ -4622,10 +4635,12 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr
 
     while(!mCurrentFrame.imuIsPreintegrated())
     {
+        // 当前帧需要预积分完毕，这段函数实在localmapping里调用的
         usleep(500);
     }
 
-
+    // frame的mpLastKeyFrame只是用于预积分（imu模式）
+    // TODO 如果上一帧正好是上一帧的上一关键帧（mLastFrame.mpLastKeyFrame与mLastFrame不可能是一个，可以验证一下）
     if(mLastFrame.mnId == mLastFrame.mpLastKeyFrame->mnFrameId)
     {
         mLastFrame.SetImuPoseVelocity(mLastFrame.mpLastKeyFrame->GetImuRotation(),
@@ -4638,12 +4653,13 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr
         Rwb1 = mLastFrame.mpLastKeyFrame->GetImuRotation();
         Vwb1 = mLastFrame.mpLastKeyFrame->GetVelocity();
         t12 = mLastFrame.mpImuPreintegrated->dT;
-
+        // 根据mLastFrame的上一个关键帧的信息（此时已经经过imu初始化了，所以关键帧的信息都是校正后的）以及imu的预积分重新计算上一帧的位姿
         mLastFrame.SetImuPoseVelocity(Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaRotation(),
                                       twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
                                       Vwb1 + Gz*t12 + Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
     }
 
+    // 当前帧是否做了预积分
     if (mCurrentFrame.mpImuPreintegrated)
     {
         twb1 = mCurrentFrame.mpLastKeyFrame->GetImuPosition();