vins博客的一部分8

optimization()

优化先验残差、重投影残差、预积分残差。（即要拟合的目标是，之前边缘化后的先验值，前后帧之间的IMU的预积分值，每一个特征点的重投影位置。）

首先通过一个函数vector2double()把vector转为double(ceres支持),然后并把Rs Ps Vs Bas…的数据提出来存到para_Pose para_SpeedBias …中

1 定义问题和核函数

ceres::Problem problem;// 定义ceres的优化问题
ceres::LossFunction *loss_function;//核函数
//loss_function = NULL;
loss_function = new ceres::HuberLoss(1.0);//HuberLoss:当预测偏差小于 δ 时，它采用平方误差,当预测偏差大于 δ 时，采用的线性误差。

2 添加优化参数

用四元数表示旋转不支持加法,通过LocalParameterization定义了新的加法和雅克比。
其成员函数加法Plus:

• 将位姿的P Q分离开,对于变化δ(位置P和角度theta)
• P直接相加
• Q是 先将theta转换为四元数，然后与Q相乘，再归一化为单位四元数.

然后将滑窗内关键帧的P(xyz) Q(xyzw) 和 V Ba Bg添加到Parameter中:

for (int i = 0; i < frame_count + 1; i++)
	problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
	problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);  

把待优化量 “相机与IMU的外参” “td” 也放到优化问题problem中,可以选择不优化:problem.SetParametecrBlockConstant(para_Ex_Pose[i]);

3 添加约束条件

• 在问题中添加边缘化约束

就是边缘化得到的结果的$J$和$e$，参见marginalize

MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks);

• 在问题中添加IMU约束

pre_integrations[j]是i~j帧间的预积分项，它包含了imu预积分的值、增量误差的雅可比、协方差矩阵、残差计算函数…一起就传进去计算残差和残差的雅可比。
将滑窗内所有的imu预积分需要的优化量，成对传入残差块，para_Pose[i]等为滑窗内的P Q，para_SpeedBias[i]为 V Ba Bg。
利用imu预积分的值等计算残差，详见IMUFactor

for (int i = 0; i < frame_count; i++)
   IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);//传入不需要优化的常量
   problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i],para_Pose[j], para_SpeedBias[j]);//每次:ij帧的 pqv 和ba bg

• 在问题中添加相机约束

将所有的特征点，每个id对应的所有帧上他的归一化平面位置、速度成对传入；
将滑窗内对应刚刚传入的特征点的帧需要的优化量 P Q 外参 逆深度 td 成对传入残差块
详见ProjectionTwoFrameOneCamFactor

for (所有帧)
    ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(2个xyz,2个u,2个td)//传入不需要优化的常量：点的位置、像素速度
    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]);

如果是双目的,对右目也计算一次重投影误差，ProjectionTwoFrameTwoCamFactor和左目完全一样,只是重投影到了右目,外参用的 IMU->右camera

4 配置优化选项求解

ceres::Solver::Options options;//优化选项
ceres::Solver::Summary summary;//优化信息
ceres::Solve(options, &problem, &summary);//求解

5 边缘化掉最老帧

构建边缘化的$H\delta x=b$，然后舒尔补得到下次的先验约束和先验残差，这里的$Hb$只和要丢弃的变量有关。

• 添加与本次要丢弃变量相关的marg约束

• 添加要marg的imu约束

• 添加视觉的marg信息

• preMarginalize()得到IMU和视觉观测对应的参数块，雅克比矩阵，残差值

• marginalize()进行舒尔补正式边缘化

详见marginalize

//最后
last_marginalization_info = marginalization_info;//本次边缘化结果作为下次的值
last_marginalization_parameter_blocks = parameter_blocks;//优化变量的递归

结束！
我感觉是这个流程：

slideWindow()

如果标志位是边缘化最老帧

• 通过一个循环,swap(A[i], A[i+1]), A包括 Rs Ps Vs Ba Bg pre_integrations dt_buf linear_acc angular_v, 将最老帧移到开头，循环的结果是,例如开始A是0123456789,变成1234567890。

• 然后，PVQBaBg最后一个等于上一个，变成1234567899，再把pre_integrations dt_buf linear_acc angular_v最末尾的清掉。

• 通过Estimator::slideWindowOld()函数及其包含的f_manager.removeBackShiftDepth(R0, P0, R1, P1)在特征点管理器f_manager中更新存储的相关帧信息(作用主要是删除最老帧的特征点)

如果要边缘化前一帧
就把倒数第二帧的值,(主要是 pre_integrations, Ps, Rs, acc, angular_v, Vs, Ba, Bg换成当前帧的值，slideWindowNew()把倒数第二帧的特征点删掉，最后重新new一个预积分项, 为下一次准备