VINS-Mono学习与程序解读(5)：estimator.processIMU
附赠自动驾驶学习资料和量产经验：链接
estimator.processIMU函数实现了IMU的预积分，它在vins_estimator文件夹下的estimator.cpp文件中。
//processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{//利用imu数据进行预积分
if (!first_imu)
{//最初first_imu = 0所以会进到这个里面
first_imu = true;//只有第一次才会进入
acc_0 = linear_acceleration;//第一次要给acc_0,gyr_0把值赋上
gyr_0 = angular_velocity;
}
if (!pre_integrations[frame_count])//同样第一次要初始化该数组.
{//第一次frame_count从0开始
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
if (frame_count != 0)//如果已经开始计数的话
{
//调用IntegrationBase类中的函数push_back
/*执行
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
并完成预积分
*/
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
/*
这里要说明一下，dt_buf是一个vector<double>类型的数组，也就是说dt_buf[frame_count]是一个
vector<double>数组，因为一帧图像对应一串imu数据！
linear_acceleration_buf是一个vector<Vector3d>类型的数组，也就说
linear_acceleration_buf[frame_count]是一个vector<Vector3d>类型的数组，因为一帧图像对应一串imu数据！
angular_velocity_buf是一个vector<Vector3d>类型的数组，也就说
angular_velocity_buf[frame_count]是一个vector<Vector3d>类型的数组
*/
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);
int j = frame_count;//涉及到的公式我们下面推导！
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - 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;
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;
}
acc_0 = linear_acceleration;//更新acc_0与gyr_0
gyr_0 = angular_velocity;
}
1.IntegrationBase中的push_back函数
该函数在vins_estimator文件夹下的factor文件夹下的integration_base.h文件中。
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);
}
该函数首先将dt，acc，gyr保存至对应的容器中，然后执行函数propagate。
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{//其作用是积分计算两个关键帧之间IMU测量的变化量：
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;//sum_dt是一段时间
acc_0 = acc_1;
gyr_0 = gyr_1;
}
从程序中可以看出propagate的核心是midPointIntegration函数。下面就来看该函数，它同样也在文件integration_base.h中。
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)
{
/*
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 //角速度偏差计算结果
*/
//ROS_INFO("midpoint integration");
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - 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; //角速度偏差不变
if(update_jacobian)
{
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;//两帧IMU数据间的平均角速度！
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; //w_x 对应的斜对称矩阵
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; //a_0_x 对应的斜对称矩阵
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; //a_1_x 对应的斜对称矩阵
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;//更新雅克比矩阵
covariance = F * covariance * F.transpose() + V * noise * V.transpose();//更新协方差矩阵
}
}
我们首先给出两帧之间 PVQ 增量的中值法的离散形式。
公式与程序的对应关系如下：
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - 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;//累积速度
下面再说状态误差雅克比与噪声雅克比矩阵。
上面我们已经完成了IMU预积分计算的推导，而要将IMU预积分运用到非线性优化中，需要建立线性高斯误差状态递推方程。
先给出在t时刻误差项的线性化递推方程：
以上的推导是在连续情况下的，下面给出离散形式。
以上就是所有公式的推导。具体可以结合源码中矩阵的索引号和每一个公式对应。
processIMU最后这部分原理不用多说，其实就是把我们前面算的在 �� 系下的状态变换到世界坐标系下。
int j = frame_count;//
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - 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;
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;
2099
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
