从零开始的VIO——Allan方差工具

1. 前言

通过《从零开始手写VIO》第二章的学习，我逐渐发现课程本身的内容并不是很难理解，老师讲的比较到位，但对我最大的困难在于环境配置。如果给我一个完整的实验环境让我修改代码、补全函数，那难度会骤降，把算法转化为实际的代码并不是一件很痛苦的事，至少能够定位那些地方可以修改，这样也好发问。然而，光是只会算法已经远远不能满足这门课的需求了，所谓的“从零开始”，就是连最基本的库、工具、依赖都得从无到有。正好借Allan方差实验，在此分享整个实验的流程以及遇到的麻烦。

2. 概述

关于具体的IMU和Allan方差理论在这里不做描述，主要讲述实验的流程。本次实验是运用Allan方差法标定IMU的随机误差，这里所用的IMU数据是贺博提供的模拟数据，IMU最关注的的两点是陀螺仪和加速度计。实际的IMU在采集数据过程中，会产生两种误差，一般分为确定性误差和随机误差。确定性误差主要包括偏置、尺度、因制造不精产生的坐标轴相互不垂直产生的误差等，随机误差主要包括高斯白噪声、偏置随机游走。那么，要研究的变量，就是陀螺仪和加速度计各自的高斯白噪声和随机游走。

3. 实验

（1）前置要求

ROS-kinetic：请按照官网的步骤执行，完成安装以及初始化catkin_ws工作空间的任务。配置完成后即可开启ROS。

roscore

ceres solver：关于Eigen和Sophus的安装，可参考之前的博客。
再进行Ceres的安装，需要cmake编译：

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.4 libgflags-dev libgoogle-glog-dev libgtest-dev
git clone https://github.com/ceres-solver/ceres-solver.git

安装Matlab2018a，没有尝试过其他版本，不知兼容性如何。

（2）主要涉及的工具

涉及到的软件包一共有4个，分别是code_utils、imu_utils、kalibr_allan、vio_data_simulation，所有解压后的包都放在catkin_ws/src路径下，方便管理。其中code_utils可以通过imu_utils的README链接获取，均可以通过git clone命令获取，也可以直接去github下载。我的建议是，每下载一个软件包就编译安装一个。

https://github.com/gaowenliang/code_utils
https://github.com/gaowenliang/imu_utils
https://github.com/rpng/kalibr_allan
https://github.com/HeYijia/vio_data_simulation/tree/ros_version

例如安装code_utils（其他同理）：

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

编译过程中遇到了一个问题：提示没有找到matlab。解决方法是找到kalibr_allan/bagconvert/cmake/FindMatlab.cmake文件，将里面的matlab路径改成自己机器上对应的路径。

（3）生成IMU数据包

找到imu_utils中的launch文件夹，新建一个imu.launch文件，并仿照其他launch文件编辑，目的是提供rostoic的信息。

cd ~/catkin_ws/src/imu_utils/launch/
vim imu.launch

imu.launch文件的内容如下，最需要注意的是imu_topic和imu_name的赋值：

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

生成IMU模拟数据包imu.bag，我在执行的时候会提示找不到vio_data_simulation，请用source命令解决。

cd ~/catkin_ws
source ./devel/setup.bash
rosrun vio_data_simulation vio_data_simulation_node

这里生成的imu.bag大小为1.1G，bag文件无法直接使用，需要使用bagconvert转化成mat文件，借助matlab查看具体的数据。找到imu.bag的路径，执行以下命令：

rosbag info imu.bag              //确认imu的topic，显示为imu
rosbag play -r 200 imu.bag       //200指的是回放频率
roslaunch imu_utils imu.launch
rosrun bagconvert bagconvert imu.bag imu

这里生成的imu.mat大小为132M，默认的路径和imu.bag一样，第一次实验只有177bytes，原因就是bagconvert的时候topic名字不对，生成了一个空mat文件。还会生成一堆txt文件以及一个yaml文件，文件名中含有mytest（之前在imu.launch中定义的imu_name）的文件就是新生成的IMU数据。

（4）生成Allan方差曲线

在生成imu.mat文件后就已经完成90%的任务了，接下来就剩数据可视化。其实后来我才意识到，imu_utils和kalibr_allan是两个不同的Allan方差工具，两个都可以使用。
imu_utils工具的使用：在imu_utils/scripts/路径下可以找到很多m文件，这里我只尝试运行了draw_allan.m，修改m文件中的路径，改成先前生成的txt文件，在matlab下运行即可。具体的数据分析需要查看yaml文件，并转化到连续时间域上（imu_utils的README中有说明）。



kalibr_allan工具的使用：道理是相通的，在kalibr_allan/matlab下找到m文件，先修改SCRIPT_allan_matparallel.m中的路径，改为imu.mat的路径，运行，这个过程会持续1个小时左右（因机器的性能而异），会在kalibr_allan/data/ 路径下生成一个名字带有result的文件，把这个路径拷贝至SCRIPT_process_result.m对应的位置，再运行该文件，同样生成Allan方差曲线。具体的数据已经在图中显示。




最后还有附加的一点：在vio_data_simulation/python_tool/ 下还有一些py文件，其中draw_trajcory.py可以绘制IMU轨迹图。

4. 总结

这个实验并没有过多的算法研究或是代码修改上的难度，一路做下来却也耗费了不少时间，其根本原因还是自己对软件包的安装、ros、matlab的使用较为陌生，但也正是通过这个机会训练一下各种工具的使用。尤其是ros这一套开源框架，回头还要多加练习。有一个优秀的编译环境，是优秀代码的前提保证。