VINS详解之IMU预积分（一）
看VINS很久了，但一直处于似懂非懂的状态，因此想借着写博客的机会结合论文对代码进行详细的梳理。第一篇就从IMU预积分开始吧。
一 、首先是接收IMU的message
1 void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) 2 { 3 *** 4 imu_buf.push(imu_msg); 5 6 *** 7 predict(imu_msg); 8 9 *** 10 pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header); 11 12 }
主要是做三个操作
1、将imu_msg push到队列当中
2、调用predict函数对当前imu的位置、朝向、速度进行计算
3、发送imu的里程计信息
1、3的操作很直观，下面主要了解一下predict函数
二、predict函数解析
1、首先是从messgae读入线性加速度和角速度
Eigen::Vector3d linear_acceleration{dx, dy, dz};
Eigen::Vector3d angular_velocity{rx, ry, rz};
这里的线性加速度、加速度是在当前的IMU坐标系下的，包含传感器的漂移和重力加速度
参见论文的公式（1）
其中带有 尖冒子 a和ω是原始的线性加速度和角速度 ， 而不带尖帽子的a和ω是移除了漂移和重力加速度的。他们都是相对于当前的IMU坐标系的。
2、位置计算
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
上面两行代码具有相同的形式，目的是获得在世界坐标系下的，移除了加速度计漂移和重力加速度的，线性加速度。
当前imu message的时间戳是t， 上一个imu message的接收时间是latest_time。
因此 alatest_time = un_acc_0
at = un_acc_1
下面三行代码根据平均加速度计算位移和速度
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc; tmp_V = tmp_V + dt * un_acc;
3、朝向计算
//latest_time 与 t之间的平均角速度 Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg; //根据旋转角度 得到当前朝向 tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
上面两行代码就是根据角速度计算旋转角度，继而得到当前IMU在世界坐标系的朝向
Utility::deltaQ函数的作用是将旋转角度转换成四元数
1 static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta) 2 { 3 typedef typename Derived::Scalar Scalar_t; 4 5 Eigen::Quaternion<Scalar_t> dq; 6 Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta; 7 half_theta /= static_cast<Scalar_t>(2.0); 8 dq.w() = static_cast<Scalar_t>(1.0); 9 dq.x() = half_theta.x(); 10 dq.y() = half_theta.y(); 11 dq.z() = half_theta.z(); 12 return dq; 13 }
它的逻辑是这样的：
假设某个旋转是绕单位向量n=[nx,ny,nz]T进行了角度为θ的旋转，那么这个旋转的四元数为：
q=[cos(θ/2), nxsin(θ/2),nysin(θ/2),nzsin(θ/2)] T
根据等价无穷小sinx~x,则sin(θ/2)~θ/2, 以及当x->0s时,cosx->1
则当θ接近于0时，q=[1, nx(θ/2)， ny(θ/2), nz(θ/2)] T
根据代码的对应关系，作者貌似是认为旋转轴为[1,1,1]T（这里我也是猜测，希望有了解的朋友可以解释一下）
