看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。

因此 a_{latest_time}  =  un_acc_0

                     a_t  =  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=[n_x,n_y,n_z]^T進行了角度為θ的旋轉，那麼這個旋轉的四元數為：

q=[cos(θ/2), n_xsin(θ/2),n_ysin(θ/2),n_zsin(θ/2)] ^T

根據等價無窮小sinx~x,則sin(θ/2)~θ/2, 以及當x->0s時,cosx->1

則當θ接近於0時，q=[1, n_x(θ/2)， n_y(θ/2), n_z(θ/2)] ^T

根據程式碼的對應關係，作者貌似是認為旋轉軸為[1,1,1]^T（這裡我也是猜測，希望有了解的朋友可以解釋一下）