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预积分及残差雅克比计算

                                                

                                得到状态变化量