VINS紧耦合优化公式及代码解析

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项.