优化先验残差、重投影残差、预积分残差。（即要拟合的目标是，之前边缘化后的先验值，前后帧之间的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(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 J J和 e e e，参见marginalize
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks);
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 δ x = b Hδx=b Hδx=b，然后舒尔补得到下次的先验约束和先验残差，这里的 H b Hb Hb只和要丢弃的变量有关。
添加与本次要丢弃变量相关的marg约束
添加要marg的imu约束
添加视觉的marg信息
preMarginalize()得到IMU和视觉观测对应的参数块，雅克比矩阵，残差值
marginalize()进行舒尔补正式边缘化
详见marginalize
//最后
last_marginalization_info = marginalization_info;//本次边缘化结果作为下次的值
last_marginalization_parameter_blocks = parameter_blocks;//优化变量的递归
结束！
我感觉是这个流程：
如果标志位是边缘化最老帧
通过一个循环,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一个预积分项, 为下一次准备