1. IMU预积分的代码部分 —— integration_base.h
（1）预积分的构造函数
IntegrationBase(const Eigen::Vector3d &_acc_0, // 初始加速度
const Eigen::Vector3d &_gyr_0, // 初始角速度
const Eigen::Vector3d &_linearized_ba, // 初始加速度零偏
const Eigen::Vector3d &_linearized_bg) // 初始角速度零偏
// 列表初始化 使用括号{}内的输入参数给对应的成员变量赋初值
: acc_0{_acc_0}, gyr_0{_gyr_0},
linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
// 初始时只有自身与自身才有联系，因此雅可比矩阵初始化为15*15的单位阵
jacobian{Eigen::Matrix<double, 15, 15>::Identity()},
// 初始时没有任何IMU数据，因此协方差矩阵为15*15的全零矩阵
covariance{Eigen::Matrix<double, 15, 15>::Zero()},
// 初始时的位移为0
sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()},
// 初始时的姿态为单位四元数
delta_q{Eigen::Quaterniond::Identity()},
// 初始时的速度为0
delta_v{Eigen::Vector3d::Zero()}
{
/*
noise对应的是前面的噪声协方差矩阵V
由于噪声n是18*3的，而V = n*n^T，故V是(18*1)*(1*18) = 18*18
下面根据n = [n_{ak}, n_{wk}, n_{ak+1}, n_{wk+1}, n_{ba}, n_{bw}]给协方差矩阵noise(V)赋初值
参数ACC_N，GYR_N，ACC_W，GYR_W是通过在parameters.cpp中读取配置文件来获取的
*/
noise = Eigen::Matrix<double, 18, 18>::Zero();
// 与i时刻的加速度噪声有关
noise.block<3,3>(0,0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
// 与i时刻的角速度噪声有关
noise.block<3,3>(3,3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
// 与i+1时刻的加速度噪声有关
noise.block<3,3>(6,6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
// 与i+1时刻的角速度噪声有关
noise.block<3,3>(9,9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
// 与噪声的加速度偏置有关
noise.block<3,3>(12,12) = (ACC_W*ACC_W) * Eigen::Matrix3d::Identity();
// 与噪声的角速度偏置有关
noise.block<3,3>(15,15) = (GYR_W*GYR_W) * Eigen::Matrix3d::Identity();
}
（2）预积分调用接口
// 每来一帧数据，就进行一次IMU预积分处理，而调用的接口就是push_back
// 关于形参，dt：当前传入的IMU数据对应的时间戳 acc：当前时刻IMU数据中加速度计的值
// gyr：当前时刻IMU数据中陀螺仪的值
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
// 相关时间差和传感器数据存入对应的buf中，方便后续repropagate(发生在VIO初始化过程中)
dt_buf.push_back(dt); // 获取传入imu数据中的时间戳
acc_buf.push_back(acc); // 获取传入imu数据中的加速度
gyr_buf.push_back(gyr); // 获取传入imu数据中的角速度
// 根据获取到的时间戳、加速度、角速度，在propagate中进行预积分
propagate(dt, acc, gyr);
}
（3）propagate()
void propagate(double _dt,
const Eigen::Vector3d &_acc_1,
const Eigen::Vector3d &_gyr_1)
{
// 将传入的参数赋值给对应的成员变量进行备份
// 方便下面完成中值积分后将当前时刻(i+1)传入的变量作为下一次的上一时刻值(i)
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
// 定义要计算的p, v, q
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
// 中值积分
midPointIntegration(
_dt, // 输入的相邻帧的时间差
acc_0, // 输入的i时刻的加速度 (加速度计读数)
gyr_0, // 输入的i时刻的角速度 (陀螺仪读数)
_acc_1, // 输入的i+1时刻的加速度 (加速度计读数)
_gyr_1, // 输入的i+1时刻的角速度 (陀螺仪读数)
delta_p, // 输入的i时刻的alpha
delta_q, // 输入的i时刻的gamma
delta_v, // 输入的i时刻的beta
linearized_ba, // 输入的加速度零偏 (在两帧图像的预积分中保持不变)
linearized_bg, // 输入的角速度零偏 (在两帧图像的预积分中保持不变)
result_delta_p, // 输出的i+1时刻的alpha
result_delta_q, // 输出的i+1时刻的gamma
result_delta_v, // 输出的i+1时刻的beta
result_linearized_ba, // 输出的i+1时刻的加速度零偏 (保持不变，输入 = 输出)
result_linearized_bg, // 输出的i+1时刻的角速度零偏 (保持不变，输入 = 输出)
1); // 是否更新雅可比的标志位 1表示要更新
// 将中值积分结果赋值给对应的成员变量
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize(); // 归一化，单位四元数才能表示旋转
// 将当前传入的值作为下一次的上一时刻值
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
（4）中值积分
注意：代码中的下标与上述理论推导公式中的不一样，这里的k表示初始的第i个图像帧，i与i+1表示初始图像帧之后的两相邻IMU时刻。
根据传入的相邻时刻IMU数据，使用中值积分进行预积分计算，代码中的变量与公式中的对应如下：
_gyr_0: 对应公式中的w_i^b，表示第i个IMU时刻的角速度
_gyr_1: 对应公式中的w_{i+1}^b，表示第i+1个IMU时刻的角速度
un_gyr: 对应公式中的\bar{w}_i^b，表示第i个IMU与第i+1个IMU的中值角速度
_acc_0: 对应公式中的a_i^b，表示第i个IMU时刻的加速度计读数 (包含偏置)
delta_q: 对应公式中的q_{b_i}^{b_k}，表示第i个IMU时刻到第k个图像帧(公式中任意第i时刻)的姿态
un_acc_0: 表示将body坐标系下第i时刻的加速度去掉偏置后转换到第k个图像帧上得到的加速度
_acc_1: 对应公式中的a_{i+1}^b，表示第i+1个IMU时刻的加速度计读数 (包含偏置)
result_delta_q: 对应公式中的q_{b_{i+1}}^{b_k}，表示第i+1个IMU时刻到第k个图像帧的姿态
un_acc_1: 表示将body坐标系下第i+1时刻的加速度去掉偏置后转换到第k个图像帧上得到的加速度
result_delta_p: 对应公式中的\alpha_{b_{i+1}}^{b_k}，表示第i+1个IMU时刻到 第k个图像帧的位移预积分量
result_delta_v: 对应公式中的\beta_{b_{i+1}}^{b_k}，表示第i+1个IMU时刻 到 第k个图像帧的速度预积分量
linearized_ba, result_linearized_ba: 第i与第i+1两相邻时刻的加速度偏置，二者相等
linearized_bg， result_linearized_bg: 第i与第i+1两相邻时刻的角速度偏置，二者相等
/*
注意：这里的k表示的是第k个图像帧，k+1表示的是第k+1个图像帧
i表示第i个imu数据，i+1表示第i+1个imu数据
由于IMU频率高于相机频率，故这两个图像帧之间会存在多个时刻的IMU数据，对应关系大致如下
k k+1
1 2 3 ... i i+1 ...
传入的是相邻时刻的imu相关量，而不是相邻关键帧之间的imu相关量
因此，将两个相邻图像帧之间的相邻imu数据记为第i时刻与第i+1时刻的imu数据
*/
// Step 1：每来一个IMU，根据中值积分更新预积分量
// Step 2：计算出矩阵F和矩阵V
// Step 3：使用矩阵F和V更新雅可比矩阵和协方差矩阵
void midPointIntegration(
_dt, // 输入的相邻帧的时间差
acc_0, // 输入的i时刻的加速度 (加速度计读数)
gyr_0, // 输入的i时刻的角速度 (陀螺仪读数)
_acc_1, // 输入的i+1时刻的加速度 (加速度计读数)
_gyr_1, // 输入的i+1时刻的角速度 (陀螺仪读数)
delta_p, // 输入的i时刻的alpha
delta_q, // 输入的i时刻的gamma
delta_v, // 输入的i时刻的beta
linearized_ba, // 输入的加速度零偏 (在两帧图像的预积分中保持不变)
linearized_bg, // 输入的角速度零偏 (在两帧图像的预积分中保持不变)
result_delta_p, // 输出的i+1时刻的alpha
result_delta_q, // 输出的i+1时刻的gamma
result_delta_v, // 输出的i+1时刻的beta
result_linearized_ba, // 输出的i+1时刻的加速度零偏 (保持不变，输入 = 输出)
result_linearized_bg, // 输出的i+1时刻的角速度零偏 (保持不变，输入 = 输出)
1) // 是否更新雅可比的标志位 为1表示要更新
{
// Step 1：每来一个IMU，根据中值积分更新预积分量
/*
delta_q表示的是第i个IMU数据到初始的第k个图像帧的姿态，对应公式中的q_{b_i}^{b_k}
_acc_0表示第i个IMU时刻的IMU加速度计读数(包含偏置)
_acc_0 - linearized_ba表示第i个IMU时刻的IMU加速度计读数(去掉偏置)
un_acc_0表示将body坐标系下第i个IMU数据去掉偏置后的加速度转换到body坐标系下第k个图像帧上，对应公式中的q_{b_i}^{b_k} * a_i^b
*/
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
// _gyr_0:第i个IMU时刻的角速度, _gyr_1:第i+1个IMU时刻的角速度
// un_gyr:第i个IMU时刻与第i+1个IMU时刻的中值角速度
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
// 代入中值角速度，根据公式R_{i+1}^k = R_i^k * R_{i+1}^i
// 即result_delta_q表示当前第i+1个IMU时刻到初始的第k个图像帧的IMU姿态(预积分量gamma)
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
/*
_acc_1表示body坐标系下第i+1个IMU时刻的IMU加速度计读数(包含偏置)
un_acc_1表示将body坐标系下去偏置后的第i+1个IMU时刻的加速度转换到初始的第k个图像帧上，对应公式中的a_{i+1}^k = q_{i+1}^k * a_{b_{i+1}}
*/
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
// un_acc: 表示将第i个IMU时刻与第i+1个IMU时刻的加速度转换到初始第k帧上的中值加速度(去掉偏置后的)
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
// result_delta_p表示代入中值加速度后计算出的当前第i+1个IMU时刻到初始的第k个图像帧的预积分量alpha
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
// result_delta_v表示代入中值加速度后计算出的当前第i+1个IMU时刻到初始的第k个图像帧的预积分量beta
result_delta_v = delta_v + un_acc * _dt;
// 加速度偏置在预积分中保持不变(第i个IMU时刻的加速度偏置与第i+1个IMU时刻的加速度偏置相等)
result_linearized_ba = linearized_ba;
// 角速度偏置在预积分中保持不变(第i个IMU时刻的角速度偏置与第i+1个IMU时刻的角速度偏置相等)
result_linearized_bg = linearized_bg;
// 按照理论推导公式，更新协方差矩阵及雅克比
if(update_jacobian)
{
// 事先计算出3个反对称矩阵
// [1/2 * (w_i + w_{i+1}) - b_{w_i}]^
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
// (a_i - b_{a_i})^
Vector3d a_0_x = _acc_0 - linearized_ba;
// (a_{i+1} - b_{a_i})^
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x << 0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x << 0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x << 0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
MatrixXd F = MatrixXd::Zero(15, 15);
// (0, 0)开始的3*3矩阵块为单位阵
F.block<3, 3>(0, 0) = Matrix3d::Identity();
// 对应推导出的系数矩阵F中的f01
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + -0.25 * result_delta_q.toRotationMatrix()
* R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
// (0, 6)开始的3*3矩阵块为delta_t
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
// 对应推导出的系数矩阵F中的f03
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() +
result_delta_q.toRotationMatrix()) * _dt * _dt;
// 对应推导出的系数矩阵F中的f04
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() *
R_a_1_x * _dt * _dt * -_dt;
// 对应推导出的系数矩阵F中的f11
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
// (3, 12)开始的3*3矩阵块为-delta_t
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
// 对应推导出的系数矩阵F中的f21
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x
* (Matrix3d::Identity() - R_w_x * _dt) * _dt;
// (6, 6)开始的3*3矩阵块为单位阵
F.block<3, 3>(6, 6) = Matrix3d::Identity();
// 对应推导出的系数矩阵F中的f23
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() +
result_delta_q.toRotationMatrix()) * _dt;
// 对应推导出的系数矩阵F中的f24
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() *
R_a_1_x * _dt * -_dt;
// (9, 9)开始的3*3矩阵块为单位阵
F.block<3, 3>(9, 9) = Matrix3d::Identity();
// (12, 12)开始的3*3矩阵块为单位阵
F.block<3, 3>(12, 12) = Matrix3d::Identity();
// 矩阵V中关于噪声项相对于推导出的相差一个负号，对于高斯白噪声而言相差一个符号并没有影响
MatrixXd V = MatrixXd::Zero(15,18);
// 对应推导出的系数矩阵V中的v00
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
// 对应推导出的系数矩阵V中的v01
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() *
R_a_1_x * _dt * _dt * 0.5 * _dt;
// 对应推导出的系数矩阵V中的v02
V.block<3, 3>(0, 6) = 0.25*result_delta_q.toRotationMatrix()*_dt * _dt;
// 对应推导出的系数矩阵V中的v03 v01 = v03
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
// (3, 3)开始的3*3矩阵块为-0.5 * delta_t
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
// (3, 9)开始的3*3矩阵块为-0.5 * delta_t
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
// (6, 0)开始的3*3矩阵块为-0.5 * Ri * delta_t
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
// 对应推导出的系数矩阵V中的v21
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() *
R_a_1_x * _dt * 0.5 * _dt;
// (6, 6)开始的3*3矩阵块为-0.5 * R_i+1 * delta_t
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
// 对应推导出的系数矩阵V中的v23 v23 = v21
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
// (9, 12)开始的3*3矩阵块为delta_t
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
// (12, 15)开始的3*3矩阵块为delta_t
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
// 更新雅可比，避免后续因零偏变化导致重新计算预积分
jacobian = F * jacobian;
// 更新协方差矩阵
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}
}
}
499
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
