vins-fusion代码解读[六] vio中processThread线程

processThread线程主体是processMeasurements（）函数，当SetParameter后，线程开启。下面均以初始化完成后，并且有准确的相机IMU外参来阅读代码。

processMeasurements（）

当featureBuf不为空的时候，函数开始进行前端的测量处理。

• td:代表相机与imu之间的时间差。

等待imu有收集到最新图像时刻的值。
接着通过getIMUInterval（）获取preTime以及curTime这两个时间段之间IMU数据，存放到accVector以及gyrVector里面。然后对每一帧IMU之间的时间差td,然后把加速度计以及陀螺仪的数据交给processIMU()函数处理。

processIMU()

第一次调用这个函数的时候，会把first_imu设置为true,同时acc_0,gyr_0设置成当前的一帧的acc,gyr的值。

• frame_count 代表当前处理的这一帧在滑动窗口中的第几个。取值范围是在0到WINDOW_SIZE之间。
• acc_0,gyr_0，预积分计算的是两帧图像之间积分的值，通过一帧帧imu帧的迭代求解来获得。通过对pre_integrations进行push_back（），可以迭代求解出最终两帧图像间预积分的值。但是要计算一个个预积分，需要知道这个预积分开头的帧当前的加速度以及角速度，以这个基准来建立的。每次使用完这个函数都会更新acc_0，gyr_0的值，即当要建立一个新的预积分的时候，这个基准直接从acc_0，gyr_0获取。

因此当滑动窗口还没有满的时候，就要new出pre_integrations[frame_count]出来。或者当slidewindow的时候，会把pre_integrations[WINDOW_SIZE]delete掉。这时候，就需要new一个新的预积分出来。

• dt_buff, linear_acceration_buf , angular_velocity_buf 三者都是frame_count里面的，即一个图像帧里面包含包含了这些东西。

同时，通过IMU的这些数据，来更新三个状态量，Ps,Vs,Rs（这个是绝对坐标系下的位姿）。这时候不是用预积分，而是用正常普通的积分并且用上中值积分。 processIMU()已经完成，接着是processImage()。

processImage()

传入的是两个参数，第一个是图像特征的描述，第二个是图像帧获取的时间

• map> 代表：<特征点id,>

首先通过视差判断最新进来这一帧的视差（旋转补偿的），如果视差偏小，我们到时候边缘化帧的时候，就把滑窗里面倒数第二新的帧给边缘化掉。如果视差足够的话，我们就把滑窗里面最旧的帧给边缘化掉。
此时，我们已经有了Ps，Rs, tic,ric(imu到相机的外参)，因此，我们可以把图像中两帧有跟踪到的2D进行三角化。此时是不准确的，因为Ps，Rs都是IMU直接积分得到的值。
接着进行optimization(),进行后端的非线性优化。
优化完成后，去除图像中残差大的特征点(removeOutlier)，并且删除特征点深度算出来负数的点（removeFailures()），进行故障检测.

• last_R, last_P: 滑窗里面最新的位姿
• last_R0, last_P0: 滑窗里面最旧的位姿

最后updateLatestStates(),注意的是里面调用了fastPredictIMU(),用来预测最新P,V,Q的姿态
-latest_p,latest_q,latest_v，latest_acc_0,latest_gyr_0最新时刻的姿态。这个的作用是为了刷新姿态的输出，但是这个值的误差相对会比较大，是未经过非线性优化获取的初始值。