第2节有讲到，在estimator_node.cpp中的process()函数中，会对与某一帧图像特征匹配的全部IMU数据进行处理，即对两两图像帧通过IMU建立预积分的约束，这个过程在processIMU()中完成。
1、processIMU()
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
Estimator类的成员函数，estimator.cpp/processIMU()
主要流程：
1、如果是系统的第一帧imu数据，则保存数据为 acc_0、gyr_0，由于dt=0 ，在计算预积分以及中值积分时都为0，不过重点是将本次IMU的测量值保存，这样下一次IMU数据到时就可以进行中值积分。
2、如果滑动窗口中当前帧frame_count图像预积分对象空，则为其创建一个预积分对象。
3、如果当前图像帧 frame_count = 0 ，则说明此时处理的是第一帧图像数据，则不需要进行预积分，因 为预积分是表示两帧图像之间的约束， frame_count != 0时 ，用imu数据更新该图像帧的预积分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity)
4、用中值积分更新当前图像帧对应的IMU PVQ数组 Ps、Vs、Rs，Ps、Vs、Rs数组保存滑动窗口中每一帧在世界坐标系中的位姿，在初始化完成之前这个数组保存的姿态是相对于参考帧的。
3.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},
jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()} // 预积分PVQ初始化为0
{
noise = Eigen::Matrix<double, 18, 18>::Zero();
// 上一时刻IMU噪声
noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
// 当前时刻IMU噪声
noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
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();
}
首先将当前时刻的imu观测保存 acc_0, gyr_0, linearized_ba, linearized_bg.
然后初始化预积分误差状态方程的jacobian为单位阵,
以及 协方差矩阵为零矩阵.
初始化预积分当前时刻状态 delta_p, delta_q, delta_v 为"零状态".
3.2、预积分的计算
processIMU()通过调用
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity)
计算预积分 。
// 计算预积分
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr); // 预积分传播
}
流程： 1、更新 dt_buf，acc_buf，gyr_buf，主要用于 repropagate()。
2、调用预积分递推函数 propagate(dt, acc, gyr)。
propagate()
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
// 中值积分
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
// checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
// 将递推得到的当前预积分量 设置为上一刻 用于递推
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;
// 记录当前IMU值
acc_0 = acc_1;
gyr_0 = gyr_1;
}
流程：
1、调用中值预积分函数 midPointIntegration() 得到
Vector3d result_delta_p: 利用当前imu数据计算预积分 P
Quaterniond result_delta_q: 利用当前imu数据计算预积分 Q
Vector3d result_delta_v: 利用当前imu数据计算预积分V
Vector3d result_linearized_ba: 加速度偏置。
Vector3d result_linearized_bg: 角速度偏置。
2、将计算的预积分结果保存到 delta_p、delta_q、delta_v、linearized_ba、linearized_bg。
3、imu数据保存为 acc_0、 gyr_0 ，为下一次预积分计算做准备， 注意这里acc_0，gyr_0 和 processIMU()里的不同 。
midPointIntegration()
从上面分析可以看到这个函数是预积分计算的核心函数.
midPointIntegration()函数完成了离散中值预积分的更新计算，以及误差状态方程 F矩阵与V矩阵的计算，最后更新预积分运动方程关于上一刻预积分的jacobian矩阵与误差状态的协方差矩阵。
在propagate()函数中调用的是-
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
函数的定义为:
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
_acc_0, _gyr_0 : 上一时刻IMU测量值
_acc_1, _gyr_1 : 当前时刻IMU测量值
delta_p, delta_q,delta_v : 上一时刻预积分量
linearized_ba, linearized_bg: 上一时刻imu偏置.
- 离散中值预积分的部分是:
// 更新离散中值预积分
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; // 认为前后两帧偏置linearized_bg不变
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2); // 预积分旋转部分更新
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
// 离散预积分
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
// 偏置不变
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
公式:
其中
- 预积分误差状态递推方程
这一部分是:
// 求出 预积分运动方程关于上一刻预积分的jacobian 以及 预积分误差状态的协方差矩阵
if(update_jacobian)
{
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
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;
// 误差状态运动方程 deltaZi+1 = F*deltaZi + V*N
// F 为jacobian矩阵
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();
//cout<<"A"<<endl<<A<<endl;
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;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian; // F的迭代
covariance = F * covariance * F.transpose() + V * noise * V.transpose(); // 当前预积分误差状态的协方差矩阵
}
这里我们构造的是离散预积分误差方程,如下:
其中
该方程具体的推导参考VINS-Mono理论学习——IMU预积分 Pre-integration （Jacobian 协方差）
可以简写为:
不过这里我们并不关心t+1时刻的预积分误差, 我们需要的是t+1时刻 jacobian矩阵F, 以及预积分误差的协方差矩阵.
初始化时, jacobian被初始化为单位阵I, 由于开始预积分误差和它的均值都为0, 所以协方差矩阵 covariance 初始为零矩阵.
当第一个预积分完成后, 求出F矩阵并且通过迭代求出jacobian矩阵:
再通过迭代求出预积分误差的协方差矩阵:
第一次预积分时, Pk为零矩阵, 因此第一次预积分误差的协方差矩阵完全由传感器噪声产生.
jacobian矩阵F和预积分误差的协方差矩阵的作用
jacobian矩阵F和预积分误差的协方差矩阵的作用如下:
jacobian矩阵
- 提供关于bias的jacobian, 如初始化, 求解陀螺仪bias - solveGyroscopeBias(),需要用到预积分旋转对偏置的jacobian,
预积分误差的协方差矩阵
- 优化的时候, 需要用到预积分协方差矩阵的逆即信息矩阵构建 残差与jacobian.
198
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
