vins-fusion代码解读[三] vio非线性优化optimization()函数
SLAM新手，欢迎讨论。
这是以往自己写的关于vins_fusion的博客：
optimization()函数内部，在主函数 vins_estimator:: rosNodeTest.cpp main函数前，由于创建了Estimator类，开启了processMeasurements()线程，该线程在初始化完成后，会调用 optimization()函数。
与vins-mono对比，vins-fusion在optimization()内部少了relocation，即不添加回环检测的残差项。
optimization()
非线性方程要求解的参数：
1.para_Pose[i]：滑动窗口内11帧的位姿，6自由度7变量表示 SIZE_POSE: 7
2. para_SpeedBias[i]：滑动窗口11帧对应的速度,ba,bg,9自由度。SIZE_SPEEDBIAS: 9
3.para_Ex_Pose[i]: 相机到IMU的外参变换矩阵，6自由度7变量表示 SIZE_POSE: 7
4.para_Td[0]: 滑窗内第一个时刻的相机到IMU的时钟差
滑动窗口内有效的特征点的深度值，数量为1000个（NUM_OF_F）,维度为1自由度
依次对应下面四段代码：
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_Td[0], 1);
当没有使用imu的时候，para_SpeedBias就没有加入待求变量中，并且把para_Pose[0]设置为常量。当我们有精确的相机到imu的外参时候，para_Ex_Pose也设置为常量。如果使用同步的VI设备的话，para_Td也作为一个常量。
即我们如果在外参已知，传感器同步的情况下，整个非线性优化求解的参数为：
- P
- Q
- V
- Ba
- Bg
非线性方程的残差项：
1.边缘化的残差项。直观上，我们认为，滑窗在滑动的过程中，因为要丢弃一些过去的帧，但是过去帧留有一些有效的信息。因此我们在现在姿态估计的过程中，还是需要考虑之前丢弃的帧的一些约束条件。观察变量由：last_marginalization_info提供。
2.imu残差项。通过imu预积分得到的数值pre_integration[j] 来作为观测变量。残差的推导直观上认为，在
para_Pose[i]，para_SpeedBias[i]
para_Pose[j]，para_SpeedBias[j]
这四个变量是正确的情况下，推算出来的pre_integration真值与实际的测量值之间的偏差。
3.视觉测量残差。以三角化的特征点一个个进行添加残差。具体过程，假设第l个特征点（在代码中用feature_index表示），第一次被第i帧图像观察到（代码中用imu_i表示），那个这个特征点在第j帧图像中（代码中用imu_j表示）的残差,即
para_Pose[imu_i]，para_SpeedBias[imu_i]
para_Pose[imu_j]，para_SpeedBias[imu_j]
para_Ex_Pose[0]
para_Td[0]
para_Feature[feature_index]
这些都是真值的情况下，可以解算出：
pts_i pts_j (暂时不确定什么意思，可能代表这个l特征点在i,j帧坐标系下的空间位置)
it_per_id.feature_per_frame[0].velocity,
it_per_id.feature_per_frame[0].cur_td, （it_per_id指的是第一次这个特征点被观测到对应的帧）
it_per_frame.velocity,
it_per_frame.cur_td（it_per_frame指的是这个特征点要计算残差的那帧）
注意以跟踪到该特征点的帧imu_i，初始值肯定是上一帧imu_j=imu_i-1来进行跟踪（因为采用了光流法跟踪，因此该点肯定在上一帧有被观测到，因此作为初始值）
上面三个残差对应了下面代码：
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
problem.AddResidualBlock(imu_factor, NULL,
para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
problem.AddResidualBlock(f_td, loss_function,
para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0],
para_Feature[feature_index], para_Td[0]);
接着solve()非线性优化求解，接着double2vector()把ceres求解出来的结果附加到滑窗内的变量中去。接着开始边缘化，分两种情况：边缘化倒数第二新的帧还是边缘化最旧的帧。
- vector2double()
----> 把现在滑窗里面的未经过非线性话的原数数据（初始值预设值），转化为非线性优化参数para_**。 - double2vector()
----> 把非线性优化参数para_**转化为现在滑窗里面的数据。