VINS中的IMU因子（一）

在这篇文章中我们分析一些VINS中对于IMU因子的处理和构建方式。首先来看一下再 estimator类中关于预积分因子的几个重要成员变量。pre_integrations 存储了滑动窗口中相邻两帧之间的预积分增量。acc_0，gyr_0则保存了当前时刻的角速度和加速度值。

IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];
Vector3d acc_0, gyr_0;

在processMeasurements()线程中，当图像到来的时候，会处理一次堆积的IMU数据，遍历所有的IMU数据，调用processIMU()函数进行处理，下面来看下该函数具体做了什么事？

void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity);

首先看下该函数的入参

• t : 为当前数据的时间戳
• dt : 为相邻两个IMU数据之间的时间间隔
• linear_acceleration : 加速度
• angular_velocity : 角速度

if (!first_imu)
{
    first_imu = true;
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

首先判断是否是第一帧IMU数据，如果是，则设置标志位，同时对前最新时刻的加速度和角速度数据进行赋值。

if (!pre_integrations[frame_count])
{
    pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, 		 Bas[frame_count], Bgs[frame_count]};
}

如果滑窗内的当前帧还没有IMU预积分，则为其创建并分配内存。这种情况会发生在处理每组IMU数据中的第一个的时候发生。

if (frame_count != 0)
{
    pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
 
    tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
}

如果不是第一帧图像，才进行积分操作，这是因为在vio中我们以第一帧图像和其前面的IMU数据是无法构成约束的。这里进行了两个操作，分别对pre_integrations[frame_count]和tmp_pre_integration进行了push_back操作，这个可不是vector的push_back操作，这里其实是对IMU进行预积分，预积分主要干了三件事，第一就是预积分增量的递推，第二则是计算雅可比，第三则是更新噪声协方差矩阵。

dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);

同时存储相关变量。这里的push_back才是vector的。

int j = frame_count;
/// 此时的 RS[j] 还表示的是上一个时刻的旋转，还没有更新，注意这里的 j 表示的图像，不是IMU数据下标，我们还假定两个图像之间的IMU零偏是固定的
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;   /// a^' = R^T * (a-g)
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
/// 这里已经通过积分对位姿进行了预测，作为优化的初始值
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix(); /// 右乘进行更新
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g; /// 这里的减g是因为vins建模的时候g为正数
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;

1. 根据上一时刻的加速度测量值计算出上一时刻的加速度 un_acc_0。
2. 利用上一时刻和当前时刻的角平均作为当前时刻的角速度。
3. 利用角速度更新滑窗中当前时刻的姿态。
4. 计算出当前时刻的加速度 un_acc_1。
5. 利用当前时刻和上一时刻的加速度平均作为当前时刻纠正后的加速度。
6. 更新滑窗中当前时刻的位置和速度。

acc_0 = linear_acceleration;
gyr_0 = angular_velocity; 

记录加速度和角速度的测量值。

注意到在进入非线性优化阶段求解VIO的时候，有这样一个判断

  if(!USE_IMU)
  	f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
  f_manager.triangulate(frame_count, Ps, Rs, tic, ric);

如果是使用IMU的情况下，则会使用IMU的递推状态作为优化的初始值，否则，也就是在纯双目模式在则会使用PNP解算的位姿作为初始值。当然，这里你也可以注释掉这句话，此时就是在双目+IMU的模式下使用PNP作为优化的初始值，那么上面的IMU递推部分则可以注释掉了。

接下来则要看一些 IMUFactor 类的实现细节。

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>

可以看到IMU残差块继承自 Ceres，这里的后面的参数代表什么意思呢，以这里的IMU因子为例：

• 15 代表的是 IMU 残差的维度，分别是 P,V,Q,Bg,Ba。
• 7 代表的是两帧之间前一帧的位置和姿态，只不过这里姿态用了四元数，所以是 3 + 4 = 7
• 9 代表的是两帧之间前一帧的 V,Bg,Ba，所以是 3 * 3 = 9 维
• 7 代表的是两帧之间后一帧的位置和姿态，只不过这里姿态用了四元数，所以是 3 + 4 = 7
• 9 代表的是两帧之间后一帧的 V,Bg,Ba，所以是 3 * 3 = 9 维

如果想要使用 Ceres 则要重载其 Evaluate 函数，用于计算残差和雅可比，下面我们来详细分析一下其实现。

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

• double const *const *parameters : 存储的是个二维数组，一共有四个数组，每个数组有 7, 9, 7, 9 个元素，和前面的 ceres::SizedCostFunction<15, 7, 9, 7, 9> 参数是对应的。
• double *residuals : 计算得到的残差。
• double **jacobians : jacobian 矩阵。

第一个参数为入参，第二三个函数为出参。

Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

首先获得 i 和 j 时刻的状态变量。

然后调用预积分类的 evaluate 函数计算残差：

Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                     Pj, Qj, Vj, Baj, Bgj);

evalueate 的函数体的内容为：

Eigen::Matrix<double, 15, 1> residuals;
/// 分别获得当前时刻 P, V, Q 对 ba,bg 的导数
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

/// Q 只和 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);

/// 得到 ba 优化前后的变化量，Bai 为上一时刻优化后的加速度零偏
Eigen::Vector3d dba = Bai - linearized_ba;
/// 得到 bg 优化前后的变化量，Bgi 为上一时刻优化后的加速度零偏
Eigen::Vector3d dbg = Bgi - linearized_bg;

/// 根据变化量和对每个状态量的雅可比矩阵更新预积分（预积分和状态变量PVQ无关，但是和 ba,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;

/// 计算残差
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;

当然还有一些关于预积分因子的雅可比的推导，我们放到下一节进行讨论。