视觉SLAM理论与实践进阶-手写VIO-2

一、Allan方差标定IMU

标定工具：imu_utils , kalibr_allan
环境：ubuntu16.04, MATLAB R2017b
1.1 安装 imu_utils 依赖项
在终端敲入如下命令: sudo apt-get install libdw(这个步骤没碰到啥问题,可能需要安装其他的依赖项,因为电脑之前跑过 orb-slam2 ,相关的依赖项已经安装好了,所以这个步骤我直接略过了!!!)
1.2 下载 code_utils 和 imu_utils(以下附上下载地址)
code_utils , imu_utils
注:我电脑之前已经建立了 ROS 工作空间,在完成作业的过程当中,我又新建了一个工作空间(其实是新建了两个工作空间,将 vio_data_simulation-ros_version 文件又存放到一个新的工作空间编译,后文会介绍编译过程)。。。
由于 imu_utils 依赖 code_utils,所以先把 code_utils 放在工作空间的 src 下面,进行编译。然后再将 imu_utils 放到 src 下面,再编译,切记不要将两者同时放到 src 目录下在编译,否则会出错。我当时忘了截图,所以编译错误就不放出来了,应该大同小异,正确的编译过程如下:

1.mkdir -p imu_utils_ws/src //在根目录下新建工作空间并在工作空间下新建 src 文件夹
2.cd imu_utils_ws/src //cd 到 src 文件下
3.git clone https://github.com/gaowenliang/code_utils.git //从网上下载 code_utils 安装包
4.cd .. //返回到上一级目录,即回到 imu_utils_ws 目录下
5.catkin_make //编译,编译成功之后往下一步
6.cd imu-calibration/src //再次进入 src 文件夹目录下
7.git clone https://github.com/gaowenliang/imu_utils.git //从网上下载 imu_utils 安装包
8.cd .. //返回上一级目录
9.catkin_make //编译

编译过程中可能会出现找不到 backward.hpp 的报错,这是路径问题,解决方法如下:
将文件 code_utils/src/sumpixel_test.cpp 中的#include "backward.hpp"改成#include “code_utils/backward.hpp”
编译成功之后截图如下:

1.3 测试 imu_utils
理论上需要先采集 imu 静止两小时数据,得到一个.bag 数据包。但是手头没有现成的imu 所以先用网上的数据集作为代替,这里提供三个我已经下载下来的链接,并以大疆的A3 为例进行测试:
xsens-MTI-100
ADIS-16448
DJIA3
步骤如下:
1).启动节点:roslaunch imu_utils A3.launch
(注意启动节点前要先在终端 cd 到 A3.launch 文件所在目录下,再运行以上命令。这是终端会打印出”wait for imu data”如下图所示)

此外我还对 launch 文件进行了修改
param name=“data_save_path”
type=“string”
value= “$(find imu_utils)/data_test0/”/

A3.launch文件修改如下：

在 imu_uitls 下新建 data_test0 文件夹用于存放测试结果。
A3.launch 文件修改如下:

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

2).打开另外一个终端,播放数据:rosbag play -r 200 imu_A3.bag(前提是已经按照上面链接将 DJI A3 的 bag 包下载下来了,1.6G)
运行得到运行以及运行结果截图如下:

标定的参数文件会存成 A3_imu_param.yaml 文件,我的标定结果是:

%YAML:1.0
---type: IMU
name: A3
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 2.1415235568863641e-03
gyr_w: 3.0215380894187953e-05
x-axis:
gyr_n: 2.2935468130400190e-03
gyr_w: 3.6042097092695718e-05
y-axis:
gyr_n: 2.1467211065538598e-03
gyr_w: 3.2039645752037166e-05
z-axis:
gyr_n: 1.9843027510652125e-03
gyr_w: 2.2564399837830969e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 2.7916922416898620e-02
acc_w: 6.3361435077828067e-04
x-axis:
acc_n: 2.3824684751052216e-02
acc_w: 5.3794490449555522e-04
y-axis:
acc_n: 2.3792589587461194e-02
acc_w: 6.0370081040671058e-04
z-axis:
acc_n: 3.6133492912182444e-02
acc_w: 7.5919733743257611e-04

此外还有一些加速度计和陀螺仪仿真的 txt 文件,用 imu_utils/script 文件夹中的 Matlab脚本绘制出来。(前提是已经安装了 Matlab,我的电脑之前已经安装了 Matlab R2017b linux版本)
在 imu_utils/scripts 文件夹下新建脚本,打开 matlab 运行脚本,陀螺仪和加速度计曲线绘制如下图所示:

1.4 vio_data_simulation-ros_version 模拟 imu 产生数据集
上一步采用网上下载的 DJI-A3 数据集做了一个测试,这一部分采用贺博的
vio_data_simulation-ros_version 工具产生 bag 文件模拟 imu 静止采集数据。
1).编译
准备工作:这个步骤我新建了一个工作空间(命名为catkin_ws_vio_data_simulation-ros_version,名字有点长,不过无关紧要),编译好 ros 工作空间,然后将vio_data_simulation-ros_version 从网上 git 到 src 文件下。修改工具包中 src 目录下的gener_alldata.cpp 数据路径,我的路径修改如下——bag.open("/home/chengjun/catkin_ws_vio_data_simulation-ros_version/src/vio_data_simulation-ros_version/data/imu.bag", rosbag::bagmode::Write);每个人的路不一样,要根据自己电脑的路径修改。如果不修改编译可能通不过。。。
编译过程:

cd ~/catkin_ws_vio_data_simulation-ros_version/
catkin_make

编译完后,生成一个节点 vio_data_simulation_node(终端最后一行会有提示),编译成功截图如下:

2).模拟生成 imu 数据包
先 source devel/setup.bash,这一步不能忘记,否则会报错。然后终端输入命令:rosrun vio_data_simulation vio_data_simulation_node 等待一会儿,终端会显示进度,产生数据包完成之后,会按照之前预设的路径存起来。如图所示:

这时就有了模拟的 imu 数据,如果用 imu_utils 标定,还需要自己写一个launch 文件。这里我仿照 A3.launch 文件写了一个测试用的 launch 文件,并将他们存放在同一个目录下,命名为 mems.launch,内容如下:

<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= "mems"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data_test/"/>
<param name="max_time_min" type="int" value= "120"/>
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>

然后根据测试步骤 1.3 imu_utils 测试过程产生标定文件,最终会产生一些.txt 和.yaml 文件如图所示:

在 imu_utils/scripts 文件夹下新建脚本,打开 matlab 运行脚本(这里需要自己写一个脚本),模拟产生的陀螺仪和加速度计曲线绘制如下图所示:

这里我与 A3 的结果做了一个对比,红色是模拟数据包产生的 allen 方差曲线,绿色为A3 数据集产生的 allen 方差曲线。
3).修改参数并重新绘制 allen 方差曲线
步骤 2 中是 vio_data_simulation-ros_version 中为修改参数的结果,现在修改 imu 仿真参数重新产生标定文件。
修改 vio_data_simulation-ros_version/src 目录下的 param.h 文件参数,将原来的陀螺仪和加速度计噪声全部改小,代码修改部分如下:

// noise
//double gyro_bias_sigma = 0.00005; //原来的参数
//double acc_bias_sigma = 0.0005; //原来的参数
double gyro_bias_sigma = 1.0e-5; //修改之后的参数
double acc_bias_sigma = 0.0001; //修改之后的参数
//double gyro_noise_sigma = 0.015;
//double acc_noise_sigma = 0.019;
// rad/s * 1/sqrt(hz) //原来的参数
// m/(s^2) * 1/sqrt(hz) //原来的参数
double gyro_noise_sigma = 0.005; // rad/s * 1/sqrt(hz) //修改之后的参数
double acc_noise_sigma = 0.009;
// m/(s^2) * 1/sqrt(hz) //修改之后的参数

产生修改参数之后的数据包,为了避免和之前生成的 imu.bag 数据包冲突,将修改参数之后的数据包改名为 imu1。数据产生过程和上述步骤一样,再次提醒记得 source 一下,还需要重新 catkin_make 一下,否则修改代码编译出来的结果还是之前的参数。其他不再赘述。生成数据包截图如下:

为了避免和之前的 launch 文件冲突,这里我新建了 launch 文件,命名为 mems1.lanuch,生成的.yaml 和.txt 文件如下:(我设置他俩的存储路径是一样的,所以和 mems 相关的文件在同一个文件加下)

在 scripts 目录下新建.m 脚本,绘制曲线如下图所示:

这里还是将 A3 作为比较对象,绿色的曲线为 A3,红色的曲线为修改参数之前的曲线,蓝色为修改参数之后的曲线。由显示结果可知参数改小之后 allen 方差标定的误差也相应减小。
1.5 kalibr_allan标定IMU
1）新建一个工作空间，将kalibr_allan工具箱git下来，在工作空间编译一下；
2）采用1.4小节中的vio_data_simulation-ros_version工具包生成.bag数据包；并将其中的src/param.h的参数设置如下：

double acc_noise_sigma=0.019; double acc_bias_sigma=0.0005;
double gyro_noise_sigma=0.015; double gyro_bias_sigma=0.00005;

3)将生成文件用matlab画出来，这个生成文件的过程有点漫长，我的电脑大概用了40分钟，中间步骤如果没问题，剩下的就交给时间去处理吧。耐性点。。。
结果如下图：

显然标定结果与设置的参数很接近，标定完成，完美。

二、中值积分与欧拉积分对比

2.1 编译vio_data_simulation-master，这里我将它与imu_utils放在同一个工作空间中，编译过程如下：

cd vio_data_simulation-master
mkdir build
cd build
camke ..
make
cd ../bin
./data_gen

此时，在vio_data_simulation-master/bin目录下会生成一些txt文件，我们需要的是imu_pose.txt和imu_int_pose.txt，然后用Python的matplotlib绘图。命令如下：

cd ../python_tool
python draw_trajctory.py

imu_pose.txt是由给定的轨迹方程和欧拉角，生成IMU的pose，imu_int_pose.txt是由给定的轨迹得到速度和加速度，再根据欧拉法和中值法得到IMU的pose，通过比较两个pose，可以得到欧拉法和中值法的效果。以下是欧拉法截图：

输入如下命令：python draw_points.py 得到如下显示图：

2.2 欧拉法与中值积分
上述2.1章节的结果是欧拉法的输出结果，其核心代码表达如下：

/// imu 动力学模型 欧拉积分
  for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];
        MotionData imupose1=imudata[i-1];

        //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;  
        Qwb = Qwb * dq;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;

中值积分输出结果会是怎样呢？精度是否会更高？中值积分核心代码如下：

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

       MotionData imupose = imudata[i];
       MotionData imupose1=imudata[i-1];

       //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 + imupose1.imu_gyro*dt )/2.0)/2.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*(imupose1.imu_acc) )+ gw)/2; 
       Qwb = Qwb * dq;
       Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
       Vw = Vw + acc_w * dt;

中值积分代码得到的结果，通过运行draw_trajcory.py脚本得到结果如下：

完整代码请点击这里获取
为定量比较欧拉法和中值积分的结果，我用matlab编写了一个rmse的脚本，结果截图如下：

统计出相应表格如下：

—	—	Qwb.w	Qwb.x	Qwb.y	Qwb.z	Pwb(0)	Pwb(1)	Pwb(2)
RMSE	欧拉法	0.0315	0.0005	0.0029	0.0001	0.0164	0.0002	0.0000
RMSE	中值积分	0.0315	0.0005	0.0029	0.0001	0.0001	0.0001	0.0000

由上表可知中值积分对位置精度了提高。。。
附上rmse.m脚本代码：

clear
close all
%%%%%%%%%%%%%%%%%%%%%欧拉法%%%%%%%%%%%%%%%%%%%%%%%%
data_imu_pose_ol=textread('imu_pose_ol.txt');
data_imu_int_pose_ol=textread('imu_int_pose_ol.txt');
p1_ol=data_imu_pose_ol(:,1:8);
p2_ol=data_imu_int_pose_ol(:,1:8);
[wd,len]=size(p1_ol);
%for j=1:len
for i=1:wd
%rmse1=sqrt(sum(p1(i,1)-p2(i,1).^2)/wd)
rmse2_ol=sqrt(sum((p1_ol(i,2)-p2_ol(i,2)).^2)/wd);
rmse3_ol=sqrt(sum((p1_ol(i,3)-p2_ol(i,3)).^2)/wd);
rmse4_ol=sqrt(sum((p1_ol(i,4)-p2_ol(i,4)).^2)/wd);
rmse5_ol=sqrt(sum((p1_ol(i,5)-p2_ol(i,5)).^2)/wd);
rmse6_ol=sqrt(sum((p1_ol(i,6)-p2_ol(i,6)).^2)/wd);
rmse7_ol=sqrt(sum((p1_ol(i,7)-p2_ol(i,7)).^2)/wd);
rmse8_ol=sqrt(sum((p1_ol(i,8)-p2_ol(i,8)).^2)/wd);
end
%end
rmse_ol=[];
rmse_ol=[rmse2_ol,rmse3_ol,rmse4_ol,rmse5_ol,rmse6_ol,rmse7_ol,rmse8_ol]
%%%%%%%%%%%%%%%%%%%%%中值积分法%%%%%%%%%%%%%%%%%%%%%%%%
data_imu_pose=textread('imu_pose.txt');
data_imu_int_pose=textread('imu_int_pose.txt');
p1=data_imu_pose(:,1:8);
p2=data_imu_int_pose(:,1:8);
%[wd,len]=size(p1);
%for j=1:len
for i=1:wd
%rmse1=sqrt(sum(p1(i,1)-p2(i,1).^2)/wd)
rmse2=sqrt(sum((p1(i,2)-p2(i,2)).^2)/wd);
rmse3=sqrt(sum((p1(i,3)-p2(i,3)).^2)/wd);
rmse4=sqrt(sum((p1(i,4)-p2(i,4)).^2)/wd);
rmse5=sqrt(sum((p1(i,5)-p2(i,5)).^2)/wd);
rmse6=sqrt(sum((p1(i,6)-p2(i,6)).^2)/wd);
rmse7=sqrt(sum((p1(i,7)-p2(i,7)).^2)/wd);
rmse8=sqrt(sum((p1(i,8)-p2(i,8)).^2)/wd);
end
%end
rmse_zz=[];
rmse_zz=[rmse2,rmse3,rmse4,rmse5,rmse6,rmse7,rmse8]

二、B-样条插值论文总结及公式推导

阅读文献如下：（论文总结和公式推导请看本人另一篇博客B-Spline)
1.Spline Fusion: A continuous-time representation for visual-inertial fusion with application to rolling shutter cameras
2.A Spline-Based Trajectory Representation for Sensor Fusion
and Rolling Shutter Cameras
3.Research Notes and Jacobians:Continuous-Time Visual-Inertial Trajectory Estimation with Event Cameras
4.Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation
5.General matrix representations for Bsplines

如果您对以上文章感兴趣可google搜索阅读（注：请善用搜索引擎）

友情提示：代码下载需要C币，请事先判断是否对您有帮助，谨慎下载哦！！！