从零手写VIO（二）

---

在这里对从零手写VIO的第二次作业进行总结：IMU代码仿真，使用使用Allen方差工具标定IMU imu_utils或kalibr_allan
这是我总结的一份从零写VIO第二讲的思维导图从零写VIO(二)

---

作业题目：

1仿真代码解析

仿真代码地址：https://github.com/HeYijia/vio_data_simulation
该github仓库中除了提供了普通版本的仿真代码外，还提供了ROS版本的仿真代码，用于生成img.bag，以供后面使用Allan方差工具进行标定。

1.1编译vio_data_simulation-master

首先是编译这个普通版本的仿真代码，就是按编译的常规套路来：

git clone https://github.com/HeYijia/vio_data_simulation
cd vio_data_simulation
mkdir build
cd build
camke ..
make
cd ../bin
./data_gen

这样就生成我们所需的：

imu_pose_noise.txt ，imu_pose.txt ，imu_int_pose_noise.txt， imu_int_pose.txt

等数据。

使用python-tool下的Python脚本，可以绘制处点，IMU轨迹等

cd ../python_tool
python draw_trajctory.py

1.2仿真思路

指定轨迹方程,求一阶导得到速度, 角速度,求二阶导得到加速度。对加速度角速度添加高斯白噪声和bias 的随机游走噪声，得到仿真数据。

1.3运动模型

在该仿真代码中，在imu.cpp中定义了IMU的运动模型：

MotionData IMU::MotionModel(double t)
{

    MotionData data;
    // param
    float ellipse_x = 15;
    float ellipse_y = 20;
    float z = 1;           // z轴做sin运动
    float K1 = 10;          // z轴的正弦频率是x，y的k1倍
    float K = M_PI/ 10;    // 20 * K = 2pi 　　由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周

    // translation world系下 因为IMU的position就是body系在world系下的坐标
    // twb:  body frame in world frame
    Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);
    Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));              // position导数　in world frame
    double K2 = K*K;
    Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));     // position二阶导数

    // Rotation 
    double k_roll = 0.1;
    double k_pitch = 0.2;
    // body系下的角度
    Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t );   // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]
    // 对欧拉角进行求导，得到world系下的欧拉角速度
    Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K);      // euler angles 的导数

//    Eigen::Vector3d eulerAngles(0.0,0.0, K*t );   // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
//    Eigen::Vector3d eulerAnglesRates(0.,0. , K);      // euler angles 的导数

    Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles);         // body frame to world frame
    Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;   //  euler rates trans to body gyro

    Eigen::Vector3d gn (0,0,-9.81);                                   //  gravity in navigation frame(ENU)   ENU (0,0,-9.81)  NED(0,0,9,81)
    Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp -  gn );  //  Rbw * Rwn * gn = gs

    data.imu_gyro = imu_gyro;   // body系下的角速度
    data.imu_acc = imu_acc;     // body系下的加速度
    data.Rwb = Rwb;             //IMU的位姿数据使用Twb进行存储
    data.twb = position;
    data.imu_velocity = dp;     // IMU的速度 world系
    data.timestamp = t;         // 时间戳
    return data;

}

可以看到，指定的IMU轨迹方程Position比较简单，大致能想出它的样子，大概是一个“王冠”的样子它在xy面投影为圆心（-5，-5）的圆。同时IMU也在以自身坐标系下做旋转运动，以欧拉角表示其旋转，该旋转也是一个随时间变化的函数。

对Position进行一阶导和二阶导，分别得到IMU在世界坐标系下的速度和加速度。对Rotation进行一阶导，得到IMU在body系下的角速度。利用这些值，我们最终得到了IMU的MotionData：

	data.imu_gyro = imu_gyro;   // body系下的角速度
    data.imu_acc = imu_acc;     // body系下的加速度
    data.Rwb = Rwb;             //IMU的位姿数据使用Twb进行存储
    data.twb = position;
    data.imu_velocity = dp;     // IMU的速度 world系
    data.timestamp = t;         // 时间戳

1.4添加噪声

设定,加速度的高斯白噪声设定为 0.019, 陀螺仪的高斯白噪声为 0.015. 加速度 bias 的随机游走噪声设定为 5e−4 ,陀螺仪的 bias 随机游走噪声设定为 5e−5 。

void IMU::addIMUnoise(MotionData& data)
{
    std::random_device rd;
    std::default_random_engine generator_(rd());
    std::normal_distribution<double> noise(0.0, 1.0);
    // gyro
    Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));// 生成一个高斯白噪声
    Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();  // 感觉这里可有可无
    // w = w + ng + bg, 其中ng = σd*n, n~N(0,1),σd = σ/sqrt(Δt)  ,bg会在下面单独更新
    data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;
    // acc
    Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
    Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();// 
    // a = a + na + ba, 其中na = σd*n, n~N(0,1),σd = σ/sqrt(Δt)  ,ba会在下面单独更新
    data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;

    // gyro_bias update
    Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));
    // bg = σd*n, n~N(0,1), σd = σ*sqrt(Δt)
    gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
    data.imu_gyro_bias = gyro_bias_;

    // acc_bias update
    Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));
    // ba = σd*n, n~N(0,1), σd = σ*sqrt(Δt)
    acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
    data.imu_acc_bias = acc_bias_;

}

在这里我们只考虑了高斯白噪声和 bias 随机游走，需要注意两者的计算：

w = w + ng + bg, 其中ng = σdn, n~N(0,1),σd = σ/sqrt(Δt) , bg = σdn, n~N(0,1), σd = σsqrt(Δt)
a = a + na + ba, 其中na = σdn, n~N(0,1),σd = σ/sqrt(Δt), ba = σdn, n~N(0,1), σd = σsqrt(Δt)

因为IMU获取的数据都是离散数据，注意高斯白噪声方差从连续时间到离散时间需要乘以一个1/sqrt(Δt)，随机游走的噪声方差从连续时间到离散之间需要乘以一个sqrt(Δt)，Δt为传感器的采样时间。

至此我们获得了IMU的仿真数据，主要是IMU在body系下的位姿数据：
imu_pose_noise.txt
imu_pose.txt

1.5运动模型的离散积分

在实际应用中，因为IMU的频率远远大于相机频率，所以我们往往需要对IMU离散数据进行积分，使IMU数据与相机数据对齐。
这次就利用上面得到的IMU的离散仿真数据，使用欧拉积分和中值积分，比较两种积分的的效果。（通过比较积分后的轨迹imu_int_pose.txt是否和积分前的轨迹imu_pose.txt重合）

1.5.1欧拉积分

代码：

for (int i = 1; i < imudata.size(); ++i) {

            MotionData imupose = imudata[i];
            //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
            Eigen::Quaterniond dq;
            Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt / 2.0;
            dq.w() = 1;
            dq.x() = dtheta_half.x();
            dq.y() = dtheta_half.y();
            dq.z() = dtheta_half.z();

            /// imu 动力学模型 欧拉积分
            Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
            Qwb = Qwb * dq;
            Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
            Vw = Vw + acc_w * dt;

            //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
            save_points << imupose.timestamp << " "
                        << Qwb.w() << " "
                        << Qwb.x() << " "
                        << Qwb.y() << " "
                        << Qwb.z() << " "
                        << Pwb(0) << " "
                        << Pwb(1) << " "
                        << Pwb(2) << " "
                        << Qwb.w() << " "
                        << Qwb.x() << " "
                        << Qwb.y() << " "
                        << Qwb.z() << " "
                        << Pwb(0) << " "
                        << Pwb(1) << " "
                        << Pwb(2) << " "
                        << std::endl;
        }

效果：

1.5.2中值积分

代码：

for (int i = 1; i < imudata.size(); ++i)
        {

            MotionData imupose_pre = imudata[i-1];
            MotionData imupose_now = imudata[i];
            MotionData imupose_mean = imudata[i];
            imupose_mean.imu_gyro = (imupose_pre.imu_gyro + imupose_now.imu_gyro) / 2.0;

            Eigen::Quaterniond dq;
            Eigen::Vector3d dtheta_half = (imupose_mean.imu_gyro) * dt / 2.0;
            dq.w() = 1;
            dq.x() = dtheta_half.x();
            dq.y() = dtheta_half.y();
            dq.z() = dtheta_half.z();

            //　imu 动力学模型　参考svo预积分论文
            Eigen::Vector3d acc_w = Qwb * (imupose_pre.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
            Qwb = Qwb * dq; //获得Qwb_k+1
            //Qwb+1
            Eigen::Vector3d acc_w1 = Qwb * (imupose_now.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
            acc_w = (acc_w + acc_w1) / 2.0;
            //a update
            Vw = Vw + acc_w * dt;
            //Pwb update
            Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

            //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
            save_points << imupose_mean.timestamp << " "
                        << Qwb.w() << " "
                        << Qwb.x() << " "
                        << Qwb.y() << " "
                        << Qwb.z() << " "
                        << Pwb(0) << " "
                        << Pwb(1) << " "
                        << Pwb(2) << " "
                        << Qwb.w() << " "
                        << Qwb.x() << " "
                        << Qwb.y() << " "
                        << Qwb.z() << " "
                        << Pwb(0) << " "
                        << Pwb(1) << " "
                        << Pwb(2) << " "
                        << std::endl;
        }

效果：

很明显，中值积分的效果要好于欧拉积分，当然，这是显而易见的，根据原理我们就能看出：

2ROS版的IMU仿真代码

2.1 下载编译

下载编译：

mkdir -p vio_sim_ws/src
cd src
git clone https://github.com/HeYijia/vio_data_simulation
git checkout ros_version
catkin_make

2.2 生成imu.bag

首先，打开catkin_ws_vio_data_simulation/src/vio_data_simulation-ros_version/src/gener_alldata.cpp，设置imu.bag的存储路径。

bag.open("/your-path/imu.bag", rosbag::bagmode::Write);

然后，启动节点，生成imu.bag。

source devel/setup.bash
rosrun vio_data_simulation vio_data_simulation_node

至此，我们得到了一个IMU仿真数据的ROS bag包。接下来利用Allan方差工具对其进行标定。

3使用Allan方差工具标定

主要是使用imu_utils和kalibr_allan进行标定。

3.1 使用imu_utils标定

3.1.1 安装imu_utils

首先，安装依赖：

sudo apt-get install libdw-dev

先编译code_utils，然后再编译imu_utils

mkdir -p imu-calibration/src
cd imu-calibration/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
cd imu-calibration/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

中间出现的问题：
错误1：找不到Eigen
我们一般通过以下命令安装Eigen：

sudo apt-get install libeigen3-dev

这样Eigen就默认安装在/usr/include/eigen3，需要在/home/jlg/imu-calibration/src/code_utils/CMakeLists.txt中注释掉find_package(Eigen3 REQUIRED)，然后添加：

include_directories(/usr/include/eigen3)

错误2：找不到backward.hpp

atal error: backward.hpp: No such file or directory

把文件code_utils/src/sumpixel_test.cpp中的#include "backward.hpp"改成#include "code_utils/backward.hpp"即可。

错误3：std::ofstream未定义

/home/***/imu-calibration/src/imu_utils/src/imu_an.cpp:69:19: error:
aggregate ‘std::ofstream out_t’ has incomplete type and cannot be
defined

打开文件imu_utils/src/imu_an.cpp，添加：

#include 

这里参考了：https://blog.csdn.net/learning_tortosie/article/details/102415313
事实上，可能是运气好，我没有碰到这些问题，但是还是记录一下，以防万一。

3.1.2 标定

1. 采集IMU数据
   这里的IMU数据就使用我们上面生成的imu.bag。当然作者也提供了几个包，可以先测试一下。
   
   可以看到，我们之前生成的img.bag，topic为imu, 大概四个小时的时长。
2. 写launch文件
   可以参考launch文件夹下作者写的几个launch文件，根据自己imu的topic和name进行修改：

>
    >
        >
        >
        >
        >
        >
    >
>

比如我这里就修改了imu_topic所对应的value：/imu，imu_name所对应的value：mems，另外要记得把launch文件名改成imu_name，我这里就是mems.launch。

3. 生成Allan方差数据
   启动刚才的launch文件

cd imu-calibration
source ./devel/setup.bash
roslaunch imu_utils mems.launch

新打开一个terminal窗口，以200倍速播放imu.bag

rosbag play -r 200 imu.bag

要注意，这里 img.bag要带上你存放的路径：yourpath/imu.bag

过一段时间（15s左右？），就会在data文件夹下生成一些数据：

以及一个mems_imu_param.yaml：

%YAML:1.0
---
type: IMU
name: mems
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.1221203233760574e-01
      gyr_w: 8.9467924142534435e-04
   x-axis:
      gyr_n: 2.1046384179740413e-01
      gyr_w: 8.0473827758366584e-04
   y-axis:
      gyr_n: 2.1360563530391058e-01
      gyr_w: 9.2214122848867288e-04
   z-axis:
      gyr_n: 2.1256661991150250e-01
      gyr_w: 9.5715821820369432e-04
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6764053623067113e-01
      acc_w: 3.6174135533268148e-03
   x-axis:
      acc_n: 2.6811296473220231e-01
      acc_w: 3.6652237104732588e-03
   y-axis:
      acc_n: 2.6789918945541225e-01
      acc_w: 3.7987720199202969e-03
   z-axis:
      acc_n: 2.6690945450439874e-01
      acc_w: 3.3882449295868896e-03

感觉标定的不是很准啊，也不知道是怎么回事。
4. 绘制Allan方差图
修改 draw_allan.m中文件路径

将这几个路径都改为刚刚生成的数据的路径，然后运行drawn_allan.m

需要注意的是，纵坐标的单位是deg/h。
对该图的理解，可以参考：
https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
https://www.vectornav.com/support/library/gyroscope
部分解释：

对图进行分析即可得到加速度和角速度的高斯白噪声和游走Bias误差
t =1 的时候索对应的纵坐标值就是高斯白噪声
t=3的时候对应的纵坐标就是游走Bias误差。

有关Allan方差技术的完整描述，请参阅：

IEEE Std 962-1997 (R2003) Standard Specification Format Guide and Test Procedure for Single-Axis Interferometric Fiber Optic Gyros, Annex C.IEEE, 2003

或参考博客：https://blog.csdn.net/baidu_41931307/article/details/102971466

3.1.3 结果分析

在上面我们经过标定得到了一个mems_imu_param.yaml文件，其中就是标定结果，我们只看avg的结果

%YAML:1.0
---
type: IMU
name: mems
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.1221203233760574e-01
      gyr_w: 8.9467924142534435e-04
   
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6764053623067113e-01
      acc_w: 3.6174135533268148e-03
   

在之前生成IMU仿真数据的时候，我们有如下设定：

设定加速度的高斯白噪声设定为 0.019, 陀螺仪的高斯白噪声为 0.015. 加速度 bias 的随机游走噪声设定为 5e−4 ,陀螺仪的 bias 随机游走噪声设定为 5e−5 。

// time
    int imu_frequency = 200;
    int cam_frequency = 30;
    double imu_timestep = 1./imu_frequency;
    double cam_timestep = 1./cam_frequency;

需要注意的是，我们仿真的时候添加的噪声是连续噪声，而imu_utils的标定结果是离散噪声（ 感谢cynophile老哥的提醒），所以要对标定结果连续化

∆t=1/200=0.005s
这里对标定结果连续化采用的公式是
高斯白噪声：
$$\sigma ={\sigma }_d\cdot \sqrt{△t}$$
bias随机游走：
$${\sigma }_b={\sigma }_{bd}\frac{1}{\sqrt{△t}}$$

来看看标定结果和我们设置的是否相同
列表格看起来更清晰一点：
陀螺仪：

陀螺仪高斯白噪声大小	实际大小	标定结果	标定结果连续化后结果
PPT默认噪声	0.015	2.1221203233760574e-01	0.015005656711529994

陀螺仪Bias随机游走大小	实际大小	标定结果	标定结果连续化后结果
PPT默认噪声	5e−5	8.9467924142534435e-04	0.012652675171973946

加速度计：

加速度计高斯白噪声大小	实际大小	标定结果	标定结果连续化后结果
PPT默认噪声	0.019	2.6764053623067113e-01	0.018925043808911142

加速度计Bias随机游走大小	实际大小	标定结果	标定结果连续化后结果
PPT默认噪声	5e-4	3.6174135533268148e-03	0.051157953078270306

从以上结果可以看出，加速度计和陀螺仪的高斯白噪声估计是较为准确的，但是Bias随机游走的大小却差了几个数量级。

改变噪声大小后，可以比较一下标定结果，具体可参考博客：https://blog.csdn.net/weixin_44580210/article/details/92846020

3.2 使用kalibr_allan标定

3.2.1 安装kalibr_allan

mkdir -p ~/catkin_ws/src
git clone https://github.com/rpng/kalibr_allan.git
cd ..
catkin_make

具体安装路径和工程名自己看着改

3.2.2 bag文件转换成mat

参考其官网github上的介绍 ，使用bagconvert将.bag转换成.mat文件：

rosrun bagconvert bagconvert your_data_path/imu.bag imu

your_data_path就是imu.bag的路径，以转换后的文件为img.mat，和imu.bag同路径下。

3.2.3 生成曲线参数文件并标定

修改~/catkin_ws/src/kalibr_allan/matlab文件夹下的SCRIPT_allan_matparallel.m中.mat路径，运行生成参数文件

这个挺耗时间的，视机器性能吧，我的小破笔记本跑了半个多小时
然后就生成了参数文件：

再改一下SCRIPT_process_results.m中load文件的路径

运行SCRIPT_process_results.m，即可对上面生成的参数文件进行处理并展示保存结果：
陀螺仪：

加速度计：

3.2.4结果分析

kalibr_allan直接在图中给出了标定结果:

accelerometer_noise_density = 0.01931403 /设定值0.019
accelerometer_random_walk = 0.00060598 /设定值5e-4
gyroscope_noise_density =0.01538619 / 设定值0.015
gyroscope_random_walk = 0.00003262 / 设定值5-e5

简单对比一下,可以发现kalibr_allan的标定结果要比imu_utils的标定结果要精确不少，而且直接给出了高斯白噪声和bias的随机游走噪声的值，更加方便，所以更推荐使用kalibr_allan进行标定。

4 扩展阅读

参考深蓝学院《从零开始手写VIO》作业二

5 总结

其实也没啥要说的了，也知道作为一个小白，也写不出什么有深度的技术文（心有b数），目前也就是记录一下学习的过程。虽然这篇文章有很多大佬已经总结过了，而且总结的远比我写的好，但是感觉还是要自己动手写一遍，就当是整理思路了吧。
这个作业整整搞了三天，忙于装库，编译，调试，踩坑，PPT翻了一遍又一遍。中间还安装了MATLAB2018，然后导致我的破小笔记本根目录爆满，又到处找扩容根目录的方法，所幸最后还是扩容成功了。虽然过程坎坷，不止一次有想砸电脑的冲动，但是最终还是有所收获。
所有的迷茫都来于知识的匮乏。而探索未知的东西又是前进的动力。愿不断前行，拨开迷雾，寻找那一道光。
终有豁然开朗的那一天的。

参考文章：
《视觉SLAM进阶：从零开始手写VIO》第二讲作业
深蓝学院《从零开始手写VIO》作业二
手写VIO-使用Allen方差工具标定IMU