1.首先确定待优化的状态变量
对应代码，优化参数为：
Vector3d Ps[(WINDOW_SIZE + 1)];（平移向量） Vector3d Vs[(WINDOW_SIZE + 1)];（速度） Matrix3d Rs[(WINDOW_SIZE + 1)];（旋转矩阵） Vector3d Bas[(WINDOW_SIZE + 1)];（加速度计偏置） Vector3d Bgs[(WINDOW_SIZE + 1)];（陀螺仪重力偏置）
Matrix3d ric[NUM_OF_CAM]; (camera->IMU)
Vector3d tic[NUM_OF_CAM];
还需要转换成ceres可以接受的参数数组，转换如下(在函数 Estimator::Vector2double()中)
for (int i = 0; i <= WINDOW_SIZE; i++) { para_Pose[i][0] = Ps[i].x(); para_Pose[i][1] = Ps[i].y(); para_Pose[i][2] = Ps[i].z(); Quaterniond q{Rs[i]}; para_Pose[i][3] = q.x(); para_Pose[i][4] = q.y(); para_Pose[i][5] = q.z(); para_Pose[i][6] = q.w(); para_SpeedBias[i][0] = Vs[i].x(); para_SpeedBias[i][1] = Vs[i].y(); para_SpeedBias[i][2] = Vs[i].z(); para_SpeedBias[i][3] = Bas[i].x(); para_SpeedBias[i][4] = Bas[i].y(); para_SpeedBias[i][5] = Bas[i].z(); para_SpeedBias[i][6] = Bgs[i].x(); para_SpeedBias[i][7] = Bgs[i].y(); para_SpeedBias[i][8] = Bgs[i].z(); } for (int i = 0; i < NUM_OF_CAM; i++) { para_Ex_Pose[i][0] = tic[i].x(); para_Ex_Pose[i][1] = tic[i].y(); para_Ex_Pose[i][2] = tic[i].z(); Quaterniond q{ric[i]}; para_Ex_Pose[i][3] = q.x(); para_Ex_Pose[i][4] = q.y(); para_Ex_Pose[i][5] = q.z(); para_Ex_Pose[i][6] = q.w(); }
double para_Pose[WINDOW_SIZE + 1][SIZE_POSE]; double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE];
2. 向ceres中添加优化变量
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);
3. 将优化量存入数组，代码如下，依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法, 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式. 分别对应marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类.
a) 添加边缘化的残差项
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info); problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks);
b)添加IMU的residual
for (int i = 0; i < WINDOW_SIZE; i++) { int j = i + 1; if (pre_integrations[j]->sum_dt > 10.0) continue; //！添加代价函数 IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]); //！注意在添加残差的组成部分，由前后两帧的[p,q,v,b]组成，在计算雅克比的时候[p,q](7),[v,b](9)分开计算 problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]); }
重点介绍IMUFactor类重写的Evaluate()，该函数定义了通过输入parameter计算residual。关键代码：
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals); residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj);
主要计算在pre_integration->evaluate()函数中进行
Eigen::Matrix<double, 15, 1> residuals; //! 对应参考文献[1]中的公式(12),求取α，β，θ的一阶近似 //! (3,9) Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA); Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG); Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG); Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA); Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG); Eigen::Vector3d dba = Bai - linearized_ba; Eigen::Vector3d dbg = Bgi - linearized_bg; Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg); Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg; Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg; //! 求取近似之后的残差，对应参考文献[1]中的公式(22)，IMU Model residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p; residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec(); residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v; residuals.block<3, 1>(O_BA, 0) = Baj - Bai; residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi; return residuals;
对应公式如下：
c)添加视觉的residual
for (auto &it_per_frame : it_per_id.feature_per_frame) { imu_j++; if (imu_i == imu_j) { continue; } //！得到第二个特征点 Vector3d pts_j = it_per_frame.point; ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]); f_m_cnt++; }
三个误差项的特点：
1）边缘化的residual：1个.
2）IMU的residual：WINDOW_SIZE个(总长度WINDOW_SIZE+1), 每相邻两个Pose之间一个IMU residual项.
3）视觉的residual：观测数大于2的特征, 首次观测与后面的每次观测之间各一个residual项.