Vector3d Ps[(WINDOW_SIZE + )];（平移向量）

Vector3d Vs[(WINDOW_SIZE + )];（速度）

Matrix3d Rs[(WINDOW_SIZE + )];（旋转矩阵）

Vector3d Bas[(WINDOW_SIZE + )];（加速度计偏置）

Vector3d Bgs[(WINDOW_SIZE + )];（陀螺仪重力偏置）

Matrix3d ric[NUM_OF_CAM]; (camera->IMU)
Vector3d tic[NUM_OF_CAM];

for (int i = ; i <= WINDOW_SIZE; i++)

    {

        para_Pose[i][] = Ps[i].x();

        para_Pose[i][] = Ps[i].y();

        para_Pose[i][] = Ps[i].z();

        Quaterniond q{Rs[i]};

        para_Pose[i][] = q.x();

        para_Pose[i][] = q.y();

        para_Pose[i][] = q.z();

        para_Pose[i][] = q.w();

        para_SpeedBias[i][] = Vs[i].x();

        para_SpeedBias[i][] = Vs[i].y();

        para_SpeedBias[i][] = Vs[i].z();

        para_SpeedBias[i][] = Bas[i].x();

        para_SpeedBias[i][] = Bas[i].y();

        para_SpeedBias[i][] = Bas[i].z();

        para_SpeedBias[i][] = Bgs[i].x();

        para_SpeedBias[i][] = Bgs[i].y();

        para_SpeedBias[i][] = Bgs[i].z();

    }

    for (int i = ; i < NUM_OF_CAM; i++)

    {

        para_Ex_Pose[i][] = tic[i].x();

        para_Ex_Pose[i][] = tic[i].y();

        para_Ex_Pose[i][] = tic[i].z();

        Quaterniond q{ric[i]};

        para_Ex_Pose[i][] = q.x();

        para_Ex_Pose[i][] = q.y();

        para_Ex_Pose[i][] = q.z();

        para_Ex_Pose[i][] = q.w();

    }

double para_Pose[WINDOW_SIZE + ][SIZE_POSE];

double para_SpeedBias[WINDOW_SIZE + ][SIZE_SPEEDBIAS];

double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE];

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);

MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);

problem.AddResidualBlock(marginalization_factor, NULL,

                        last_marginalization_parameter_blocks);

for (int i = ; i < WINDOW_SIZE; i++)

{

    int j = i + ;

    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]);

}

Eigen::Map<Eigen::Matrix<double, , >> residual(residuals);

residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,

                                    Pj, Qj, Vj, Baj, Bgj);

Eigen::Matrix<double, , > residuals;

//! 对应参考文献[1]中的公式(12),求取α，β，θ的一阶近似

//! (3,9)

Eigen::Matrix3d dp_dba = jacobian.block<, >(O_P, O_BA);

Eigen::Matrix3d dp_dbg = jacobian.block<, >(O_P, O_BG);

Eigen::Matrix3d dq_dbg = jacobian.block<, >(O_R, O_BG);

Eigen::Matrix3d dv_dba = jacobian.block<, >(O_V, O_BA);

Eigen::Matrix3d dv_dbg = jacobian.block<, >(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<, >(O_P, ) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;

residuals.block<, >(O_R, ) =  * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();

residuals.block<, >(O_V, ) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;

residuals.block<, >(O_BA, ) = Baj - Bai;

residuals.block<, >(O_BG, ) = Bgj - Bgi;

return residuals;

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[], para_Feature[feature_index]);

    f_m_cnt++;

}