VINS代码笔记

         estimator_node.cpp系统入口

         首先找到main函数,读取参数,设置参数,

 imu_callback: 

 IMU测量的回调函数

 imu_msg [接受到的IMU消息]

          1. 执行con.notify_one();唤醒作用于process线程中的获取观测值数据的函数(怎么涉及到process线程的?)

          2. predict(imu_msg);对单次的IMU测量值做积分得到位移和姿态.      //imu_msg [采样时间内单次IMU测量值]

                2.1  un_acc_0, un_acc_1,un_gyr根据加速度计的观测值可得.然后采用中值//tmp_Q是local-->world 

                 2.2 tem_P和tem_V即下面的公式所得:

                

                      (这里的计算得到的pvq是估计值，注意是没有观测噪声和偏置的结果.)///作用是与下面预积分计算得到的pvq（考虑了观测噪声和偏置）做差得到残差。  这句话在下面做回应.

  raw_image_callback:

  只有进行闭环检测的时候才用到图像

  img_msg [回调的图像feature_callback

feature_callback:

   关键点回调函数]
   feature_msg [订阅的关键点]

process函数:

        1. getMeasurements()

                1.1 如果最新的IMU的数据时间戳小于最旧特征点的时间戳，则等待IMU刷新；如果最旧的IMU数据的时间戳大于最旧特征时间戳，则弹出旧图像

                1.2 这里IMU和Feature做了简单的对齐，确保IMU的时间戳是小于图像的 ; 在IMU buff中的时间戳小于特征点的都和该帧特征联合存入

       2. 遍历获取的Features和IMU测量数据,

     send_imu  (imu_msg [IMU数据])

                2.1  processIMU(dt ,linear_acceleration ,angular_velocity)

                         2.1.1 当滑窗不满的时候，把当前测量值加入到滑窗指定位置

                         2.1.2 残差和雅可比矩阵、协方差矩阵保存在pre_integrations中 (这里进入了头文件)

                 propagate (_dt[时间间隔]; _acc_1 [加速度计数据]; _gyr_1 [陀螺仪数据])            

                                          里面主要为  midPointIntegration 函数即:   中值法进行预积分

                                               最终的矩阵形式如下:具体推导见IMU预积分及残差雅克比计算

                                                

                                得到状态变化量result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新协方差矩阵和雅可比矩阵

                           2.1.3 提供imu计算的当前旋转，位置，速度，作为优化的初值

        3.  (跳回process函数了)

            (这一块不理解)

      4. processImage(image [特征点] , header [帧头]) 特征点处理程序

             4.1  addFeatureCheckParallax

        向Featuresmanger中添加Features并确定共视关系及视差角的大小

                 4.1.1 FeaturePerFrame 特征点在某个图像帧下的坐标（归一化平面坐标）

                 4.1.2 遍历特征点,看该特征点是否在特征点的列表中,如果没在,则将存入到Feature列表中

                         如果该Feature之前被观测到过,统计数目

                  4.1.3 计算共视关系， parallax_num为满足要求的Feature的个数

                          4.1.3.1 至少有两帧观测到该特征点(起始帧要小于倒数第二帧，终止帧要大于倒数第二帧，这样至少有三帧观测到该Feature(包括当前帧))

                         4.1.3.2 判断观测到该特征点的frame中倒数第二帧和倒数第三帧的共视关系 实际是求取该特征点在两帧的归一化平面上的坐标点的距离ans

                         4.1.3.3 得到视差总和并统计个数

                   4.1.4 得到平均视差,平均视差视差要大于某个阈值, MIN_PARALLAX=10，大约是10个像素点.  

                这样的帧即是关键帧,边缘化最老的帧.否则不是关键帧,边缘化第二帧

         4.2 将图像数据和时间存到图像帧类中：首先将数据和时间保存到图像帧的对象imageframe中（ImageFrame对象中包含特征点，时间，位姿R，t，预积分对象pre_integration，是否是关键帧）,同时将临时的预积分值保存到此对象中（这里的临时预积分初值就是在前面IMU预积分的时候计算的,即2.1.2），然后将图像帧的对象imageframe保存到all_image_frame对象中（imageframe的容器），更新临时预积分初始值

        4.3 标定相机和IMU的外参数：接着如果没有外部参数就标定外部参数，参数传递有的话就跳过这一步（默认有，如果是自己的设备，可以设置为2对外参进行在线标定）

             主要是CalibrationExRotation函数,

        corres[一组匹配的特征点] ,delta_q_imu即k ==> k+1,calib_ric_result [Camera与IMU的外参之旋转量]

                 4.3.1 滑窗内帧数加1

                 4.3.2 solveRelativeR,这是sfm中根据一组匹配点,求解出两帧的之间的变换矩阵 添加到Rc中

                 4.3.3 将IMU预积分出的旋转四元数转换成矩阵形式

                 4.3.4 求取估计出的相机与IMU之间旋转的残差

                 4.3.5 计算外点剔除的权重

                 4.3.6 求取系数矩阵 即:相机对极关系得到的旋转q的左乘和IMU预积分得到的旋转q的右乘

                 4.3.7 通过SVD分解，求取相机与IMU的相对旋转 

            解为系数矩阵A的右奇异向量V中最小奇异值对应的特征向量

                 4.3.8 判断对于相机与IMU的相对旋转是否满足终止条件

         //！1.用来求解相对旋转的IMU-Cmaera的测量对数是否大于滑窗大小。
         //！2.A矩阵第二小的奇异值是否大于某个阈值，使A得零空间的秩为1

                 这部分在推导在初始化中相对旋转部分

       4.4 判断是否需要初始化.

              4.4.1 滑窗中的Keyframe达到指定大小的时候，才开始优化

              4.4.2  初始化initialStructure()

                       4.4.2.1 保证IMU充分运动，通过线加速度判断，一开始求all_image_frame中所有帧的平均加速度, 并计算加速度的方差,q加速度的标准差（离散程度）判断保证IMU充分运动，加速度标准差大于0.25则代表imu充分激励，足够初始化.

                      纯视觉的初始化 sfm { 4.4.2.2 遍历滑窗内所有的Features

                       4.4.2.3 relativePose 在滑窗中寻找与最新的关键帧共视关系较强的关键帧

                                   4.4.2.3.1 getCorresponding 选取两帧之间共有的Features

                                   4.4.2.3.2 共视的Features应该大于20

                                   4.4.2.3.3 求取所有匹配的特征点的平均视差,

                               4.4.2.3.4 视差大于一定阈值，并且能够有效地求解出变换矩阵 solveRelativeRT 中利用cv::findFundamentalMat计算基础矩阵用5点法来得到变换矩阵

                                 这里找到的关键帧设置为l帧

                     4.4.2.4 sfm.construct

                      

                                 4.4.2.4.1 将第l帧当做原点，建立坐标系,然后将Frame转换到同一坐标系下。

                                 4.4.2.4.2 获得最后一帧到l帧的变换关系

                                 4.4.2.4.3 获得世界坐标系到l帧的变换关系

                                 4.4.2.4.4 求取世界坐标系到最后一帧到的变换关系

                                 4.4.2.4.5 先第l帧和最后一帧做三角化,根据三角化的点PnP求解第l+1的位姿,再l+1和最后一帧三角化,再PnP求l+2位姿,依次求到倒数第二帧的位姿

                                 4.4.2.4.6 第l+1帧~倒数第二帧的所有帧与第l帧三角化剩下的特征点

                                 4.4.2.4.7 从l帧开始往第一帧，逐渐帧pnp，再与第l帧进行三角定位得到更多的三维点

                                 4.4.2.4.8 剩下的那些还没有被三角定位的特征点，通过它被观察到的第一帧和最后一帧进行三角定位。

                                 4.4.2.4.9 BA优化，使用的是ceres优化位姿和特征点，residuals用的特征点到每一帧重投影误差, 将求解结果写出到q,T和sfm_tracked_points

                 4.4.2.5 solve pnp for all frame 用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值。

                4.4.2.6 if (visualInitialAlign())  进入视觉与IMU的对齐,联合初始化

                           4.4.2.6.1 VisualIMUAlignment

                                          4.4.2.6.1.1 solveGyroscopeBias(all_image_frame, Bgs); 修正陀螺仪的bias

                                        

                                                   4.4.2.6.1.1.1 公式的右侧

                                                   4.4.2.6.1.2 将残差移动到右侧 即:

                                                   4.4.2.6.1.3 JTJ·bw = 2JT(r_bkbk+1^' × q_cb0' × q_cb1).vec

                                                   4.4.2.6.1.4   sum(A)bw = sum(b)

                                                   4.4.2.6.1.5 使用Cholesky分解，求解

                                                   4.4.2.6.1.6 将求得的陀螺仪偏置δbw是变化量, 所以将每帧的Bgs[i]都加上该delta量进行更新，最后在利用更新后的gyro bias重新repropagate.

                                      4.4.2.6..1.2  LinearAlignment(all_image_frame, g, x) // 初始化速度、重力向量及尺度因子

                                            即求解:

                                                      4.4.2.6..12.1 统计状态量

                                                   4.4.2.6.1.2.2 得到 

                                                                          和

                                                     4.4.2.6.1.2.3 求解超定方程

                                                    4.4.2.6.1.2.4 解出尺度因子s  其中:

                                                   4.4.2.6.1.2.5 解出重力向量

                                                  4.4.2.6.1.2.6 细化重力向量RefineGravity

其实还是解上一步的方程,只不过多了一个g0                                                        

                                                            4.4.2.6.1.2.6.1 lxly = TangentBasis(g0);e_g的正切空间的两个正交单位向量.

                                                            4.4.2.6.1.2.6.2 e_g是上一步求出的的方向向量(单位向量),

                                                            4.4.2.6.1.2.6.3 G固定大小9.8

                                                            4.4.2.6.1.2.6.4 将式子代入到 LinearAlignment中优化

                                                            4.4.2.6.1.2.6.5 重复直至收敛

回到visualInitialAlign    4.4.2.6.2 将滑窗内的视觉帧全部设为关键帧

                                    4.4.2.6.3 getDepthVector 读取特征点的逆深度 逆深度为-1??

                                    4.4.2.6.4 三角化没有恢复出深度的特征点

                                    4.4.2.6.5 根据初始化得到的陀螺仪Bias，在滑窗内重新做一次预积分 repropagate

                                    4.4.2.6.6 Ps[i]猜测为没有尺度的两帧之间的位移

                                     

                                   4.4.2.6.7 得到滑窗内关键帧的速度(这个也不知道咋得来的)

                                   4.4.2.6.8 为滑窗内特征点的深度值加上尺度

                                   4.4.2.6.9 将重力g及滑窗内的状态量转到后面的世界坐标系下

回processImage

       4.5 solveOdometry(); 到了后端优化部分了.