从零开始手写VIO第二章作业（一步一截图，新手友好型）

前言·与同主题博文的不同

延续该系列第一篇博文的风格，本博文站在之前同主题优秀博文的肩膀上的同时，尽量补充对于新手朋友来说比较重要的细节，如文件所在路径截图，前后步骤之间的联系等。另外还会给出一些小编认为很重要但是前人博文中忽略或者存在争议的点。

1.设置 IMU 仿真代码中的不同的参数，生成 Allen 方差标定曲线

课程给了两个标定工具，imu_utils和kalibr_allan，相对于需要先在Linux环境下安装好Matlab才能正常使用的kalibr_allan来说，imu_utils的环境搭建比较省时省力，能够让新手朋友将更多的精力放到对理论和代码的研究中去。另外，kalibr_allan可以直接得到最终形式的标定结果，imu_utils需要对标定结果进行转换后才能得到最终的标定结果，这个步骤有助于新手更深入地理解理论。出于这两点，小编在这里选用imu_utils来完成对IMU参数的标定与Allen方差标定曲线的生成。

1.1环境搭建

1）ROS。因为imu_utils和kalibr_allan都是 ROS package tool所以，需要先安装ROS。高翔老师在《视觉 SLAM 十四讲》推荐Ubuntu14.04，所以小编用Ubuntu14.04这个系统。大家当然可以用其它版本的系统。这里推荐两个安装ROS的博文，能够帮助新手少走些弯路，第一：装ros慢怎么办，只需要少执行一步就可以了，第二：Ubuntu14.04安装ROS。建议先浏览第一个博客，之后详细看第二个博客。不过这两个博客可能不能涵盖安装ROS中遇到的所有问题，遇到具体问题查找相对应的攻略即可。
2）ceres。因为后续安装的code_utils依赖于ceres，所以需要先安装好ceres。ceres是《视觉 SLAM 十四讲》中需要安装的库之一，这里建议有时间把《视觉 SLAM 十四讲》中需要的库都装好。《视觉SLAM十四讲》中各库的安装。
3）code_utils。终于到了最相关的工具了！
编译顺序：imu_utils依赖code_imu。先把code_utils放在工作空间的src下面，进行编译。然后再将imu_utils放到src下面，再编译。
首先安装code_utils：

// 这里默认已经搭建好了工作空间文件夹“~/catkin_ws/src”。
cd ~/catkin_ws/src 
git clone https://github.com/gaowenliang/code_utils 
cd ~/catkin_ws 
//注意按照下面的提示修改sumpixel_test.cpp之后再编译
catkin_make

代码修改：在code_utils下面找到sumpixel_test.cpp（文件路径如下图），修改#include "backward.hpp"为 #include “code_utils/backward.hpp”，再编译。否则报错:code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory.

之后下载、编译imu_utils：

cd ~/catkin_ws/srcgit clone https://github.com/gaowenliang/imu_utils
cd ~/catkin_ws
//注意按照下面的提示修改imu_an.cpp之后再编译
catkin_make

代码修改：在imu_an.cpp（文件路径如下图）中添如下加头文件，再编译。

”#include <fstream>

否则报错:catkin_ws/src/imu_utils/src/imu_an.cpp:68:19: error: aggregate ‘std::ofstream out_t’ has incomplete type and cannot be defined，std::ofstream out_t;

到此标定工具imu_utils环境搭建完成。

1.2生成 imu.bag

首先要说明的是，从代码编写以及编译这个角度来说“生成 imu.bag ”和“imu_utils环境搭建”并没有直接联系，是两个完全独立的过程；从IMU参数标定和Allan方差曲线生成过程的角度来说，首先需要“生成 imu.bag ”，然后再利用安装好的标定工具imu_utils对生成的数据进行处理，完成参数标定和曲线生成。
方便起见，将老师给的文件夹《vio_data_simulation-ros_version》与前面由git clone命令下载到的文件放到同一个工作目录下（最终目的是为了使用 imu.bag，《vio_data_simulation-ros_version》也可以放到自己想放的其它地方），如下图：

后面的命令要求imu.bag（这里还没有生成，后面会生成这个文件）处于如下路径：
修改文件gener_alldata.cpp中的代码（如下图1,2,3）：bag.open("./imu.bag", rosbag::bagmode::Write);使得生成的imu.bag默认处于上图中的路径。如果不想修改代码，只需要在文件生成后按照原来代码中的路径找到生成的imu.bag文件，放到上图所示的路径中即可。

编译源码：

cd ~/catkin_ws
catkin_make

然后，启动节点，生成imu.bag（生成的文件所在位置见前文）：

source devel/setup.bash
rosrun vio_data_simulation vio_data_simulation_node

这里要说明的是，生成的用于标定的数据包imu.bag在仿真时设定的参数文件以及设定值，具体信息如下图1,2,3,4，1对应的是文件路径，2对应的是仿真时长，3对应的是传感器Bias的随机游走（术语是Rate Random Walk），4对应的是传感器的高斯白噪声。

使用代码参看imu.bag的topics：

rosbag info imu.bag

结果如下图 ，其中仿真时间与上文中的时长参数相一致，topics为imu，这个信息后文中的.launch文件需要用到。

1.3 使用imu_utils标定IMU的白噪声和零偏不稳定性

这里需要特别注意小标题中的“零偏不稳定性”（术语是Bias Instability或者Bias Stability），与前面设置噪声参数时设定的传感器Bias的随机游走（术语是Rate Random Walk）是两个概念。后文会详细区别这两个概念，这也是imu_utils和kalibr_allan生成结果存在区别的地方。
写launch文件，进入如下图所示路径，复制16448.launch文件，将文件名改为my.launch，打开 my.launch，将其中的imu_topic与imu_name分别改为imu（前文提到过）和mytest。

也可直接复制下面的代码：

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu"/>
        <param name="imu_name" type="string" value= "mytest"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

备份后，清空data文件夹：
之后的标定过程会生成文件于如下路径中，为了更清楚地明白生成了哪些文件，可以先把这个文件夹中的文件复制到其它地方，清空这个文件夹。

下面就是见证奇迹的时刻：
1）在桌面打开一个terminal，执行roscore，一直保持打开状态，如下图

2）后面的话多读两遍！之后打开两个terminal，都进入~/catkin_ws目录，并且其中一个输入命令rosbag play -r 500 imu.bag后先不执行，另一个执行source ./devel/setup.bash之后，输入命令roslaunch imu_utils my.launch后也先不执行，代码和过程如下：

//打开一个新的terminal
rosbag play -r 500 imu.bag 
//再打开一个新的terminal
source ./devel/setup.bash//重要！！！否则报错
roslaunch imu_utils my.launch

具体操作如下截图：

之后先执行rosbag play，后执行roslaunch，就是让两者尽可能的同步，就是不要让rosbag play执行完再去执行roslaunch！
执行结束之后，会发现之前清空的data文件夹中新增了17个文件：

打开文件mytest_imu_param.yaml，部分文件内容如下，这就是标定结果，这里应该格外注意单位rad/s和m/s^2。imu_utils默认给出了IMU的白噪声和零偏不稳定性（这里术语选用Bias Stability）这两个参数的标定结果。而我们设置的噪声参数以及kalibr_allan标定的结果都是IMU的白噪声和传感器Bias的随机游走（术语是Rate Random Walk）。至于如何用imu_utils给出传感器Bias的随机游走（Rate Random Walk）不在这里论述。

%YAML:1.0
type: IMU
name: mytest
Gyr:
   unit:  rad/s
   avg-axis:
      gyr_n: 3.5202631086455877e-01
      gyr_w: 1.9521263906414701e-03
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 4.0119905300217046e-01
      acc_w: 8.0268979565987168e-03

虽然我们得到了标定结果，但这个标定结果并不是我们最终的结果。现在得到的结果的单位是rad/s和m/s^2，而老师param.h代码中给出的单位如下面代码中最后两行，后面多了/sqrt(hz)。很多博文中认为这是从连续到离散的转换，其实不是，噪声是个能量概念或者说功率概念，我们还要把标定得到的参数归一化到每单位sqrt(hz)尺度下（这里更深的概念小编也讲不清楚，但是这里完全不涉及连续到离散的转换，所有的参数都是连续意义下的，也欢迎大家在评论区留言讨论）。

    int imu_frequency = 200;
    int cam_frequency = 30;
    double imu_timestep = 1./imu_frequency;
    double cam_timestep = 1./cam_frequency;
    double t_start = 0;
    double t_end = 3600 * 4;  // 4 hours for allan

    // noise
    double gyro_bias_sigma = 0.00015;
    double acc_bias_sigma = 0.0015;

    double gyro_noise_sigma = 0.025;    // rad/s * 1/sqrt(hz)
    double acc_noise_sigma = 0.029;      //　m/(s^2) * 1/sqrt(hz)

这里补充一下，那离散的步骤体现在哪里呢，离散的步骤体现在程序的中间过程，输入和输出都是连续意义下的参数。下面是体现离散过程的老师给出的代码片段，重点看sqrt( param_.imu_timestep )：

    Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));
    Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();
    data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;

    Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
    Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();
    data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;

从param.h代码中可以看出IUM的频率是200Hz，转换只需要每个量除以sqrt（200）,转换后的值与程序中的设定值比较如下，因为没得到给定的传感器Bias的随机游走（Rate Random Walk），这里只比较IMU的白噪声，可以看出标定效果不错。

  		unit:  		rad/s						rad/s * 1/sqrt(hz)				给定值
      gyr_n: 3.5202631086455877e-01				0.024892						0.025
      gyr_w: 1.9521263906414701e-03				1.380361808555803e-04
   		unit: 		m/s^2						m/(s^2) * 1/sqrt(hz)
      acc_n: 4.0119905300217046e-01				0.028369						0.029
      acc_w: 8.0268979565987168e-03				5.675873977003394e-04

下面是使用MATLAB画Allan方差图（小编是在window系统下完成的），将前面提到的文件夹data（新增17个文件）和imu_utils工具给定的scripts文件夹放到同一目录之下：

这里以生成陀螺仪的Allen方差图为例，修改scripts中的draw_allan.m代码（修改所要读取的文件名）：

clear 
close all

dt = dlmread('../data/data_mytest_gyr_t.txt');         
data_x = dlmread('../data/data_mytest_gyr_x.txt'); 
data_y= dlmread('../data/data_mytest_gyr_y.txt'); 
data_z = dlmread('../data/data_mytest_gyr_z.txt'); 
data_draw=[data_x data_y data_z] ;

data_sim_x= dlmread('../data/data_mytest_sim_gyr_x.txt'); 
data_sim_y= dlmread('../data/data_mytest_sim_gyr_y.txt'); 
data_sim_z= dlmread('../data/data_mytest_sim_gyr_z.txt'); 
data_sim_draw=[data_sim_x data_sim_y data_sim_z] ;


figure
loglog(dt, data_draw , 'o');
%loglog(dt, data_sim_draw , '-');
xlabel('time:sec');                
ylabel('Sigma:deg/h');             
%legend('x','y','z');      
grid on;                           
hold on;                           
loglog(dt, data_sim_draw , '-');

运行，得到陀螺仪的Allan方差图如下：

同理，得到加速度的Allan方差图如下：

包括前面我们留下的传感器Bias的随机游走（Rate Random Walk）在内的所有IMU参数，都可以通过分析这Allan方差图得到。具体可以参考论文Allan Variance. “Noise Analysis for Gyroscopes”. In: Freescale Semiconductor Document Number: AN5087 Application Note Rev. 0 2 (2015).

2.将 IMU 仿真代码中的欧拉积分替换成中值积分

2.1理论依据

给出课程所给PPT中对应的理论：

2.2代码实现

1.欧拉法
直接编译运行老师所提供的的代码，具体过程如下，需要注意的是老师所给画图代码的名字是“draw_trajcory.py”，应该是不小心把单词打错了，调用的时候用Tab键补全命令名字比较方便。

cd vio_data_simulation-master
mkdir build
cd build
cmake ..
make
cd ../bin
./data_gen
cd ../python_tool
python draw_trajcory.py

老师所给欧拉法的代码

        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();
        dq.normalize();
        
        /// 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;

仿真结果如下：

2.中值法
这里有几点需要特别说明：
a）源程序中的循环计数变量i是从1开始的，而不是从0开始的；
b）与理论中的公式相比，源程序中给出的数据是已经去掉bias之后的量；
c）必须先更新姿态矩阵，因为求中值法中求acc时，需要用到前后两个时刻的姿态四元数；
d）由陀螺数据构造出的四元数不要忘了归一化；
e）最好是在源程序的基础上做最轻微的改动。
用下面的代码替换掉老师给出的imu.cpp文件中对应的代码，编译过程同上：

        MotionData imupose_ = imudata[i-1];//上一时刻的数据
        MotionData imupose = imudata[i];//下一时刻的数据
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half = (imupose_.imu_gyro+ imupose.imu_gyro)* dt / 4.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();//这里要记得归一化
        Eigen::Vector3d acc_w = (Qwb * dq * (imupose.imu_acc) + gw + Qwb * (imupose_.imu_acc) + gw)/2;//这里注意下一时刻对应的转换矩阵是Qwb * dq，而不是Qwb
        Qwb = Qwb * dq;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;

仿真结果如下：

对比两个仿真结果图，欧拉法最后两个曲线明显分离，而中值法最后两个曲线完全重合在一起。说明中值法的精度高于欧拉法。