点击上方“
计算机视觉life
”，选择“星标”
快速获得最新干货
VIO 中,如果在世界坐标系中对 IMU 进行积分,积分项中包含体坐标系相对于世界坐标系的瞬时旋转矩阵。然而,在优化位姿时,关键帧时刻体坐标系相对于世界坐标系的旋转矩阵会发生变化,那么需要对 IMU 重新进行积分。预积分就是为了避免这种重复积分。IMU 预积分将参考坐标系改为前一帧的体坐标系,从而积出了两帧之间的相对运动。
预积分
将第 k 帧和第 k+1 帧之间的所有 IMU 进行积分,可得第 k+1 帧的位置、速度和旋转(PVQ)，作为视觉估计的初始值，示意图如下：
（点击图片放大看公式
）
连续形式
离散形式
//采用的是中值积分的传播方式
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;
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;
（左右滑动试试）
F:
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
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;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() +
result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -
_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
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;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() +
result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
（
左右滑动试试
）
V:
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3
>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt *
0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 *
_dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
（
左右滑动试试
）
离散形式的 PVQ 增量误差的 Jacobian 和协方差
对应代码在integration_base.h文件的
midPointIntegration
()：
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
从零开始学习三维视觉核心技术SLAM，扫描查看介绍，3天内无条件退款
早就是优势，学习切忌单打独斗，这里有教程资料、练习作业、答疑解惑等，优质学习圈帮你少走弯路，快速入门！
交流群
欢迎加入公众号读者群一起和同行交流，目前有
SLAM、
算法竞赛、
图像检测分割、人脸人体、
医学影像、
自动驾驶、
综合
等微信群（以后会逐渐细分），
请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。
请按照格式备注，否则不予通过
。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送
广告
，否则会请出群，谢谢理解~
推荐阅读
从零开始一起学习SLAM | 为什么要学SLAM？
从零开始一起学习SLAM | 学习SLAM到底需要学什么？
从零开始一起学习SLAM | SLAM有什么用？
从零开始一起学习SLAM | C++新特性要不要学？
从零开始一起学习SLAM | 为什么要用齐次坐标？
从零开始一起学习SLAM | 三维空间刚体的旋转
从零开始一起学习SLAM | 为啥需要李群与李代数？
从零开始一起学习SLAM | 相机成像模型
从零开始一起学习SLAM | 不推公式，如何真正理解对极约束?
从零开始一起学习SLAM | 神奇的单应矩阵
从零开始一起学习SLAM | 你好，点云
从零开始一起学习SLAM | 给点云加个滤网
从零开始一起学习SLAM | 点云平滑法线估计
从零开始一起学习SLAM | 点云到网格的进化
从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
