vio作业（二）

vio课程作业（二）

基础作业

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

allan方差工具:

• https://github.com/gaowenliang/imu_utils
• https://github.com/rpng/kalibr_allan

解答

1. imu_utils工具

编译相关工具包

• 安装依赖

sudo apt-get install libdw-dev

• 下载程序关联代码包code_utils，放到ROS工作空间src中编译，imu_utils依赖code_utils，所以这两个功能包不能同时编译，在编译imu_utils之前编译code_utils。这里有个bug，编译时会报错：

code_utils-master/src/sumpixel_test.cpp:2:24: fatal error:ackward.hpp: No such file or directory

在code_utils下面找到sumpixel_test.cpp，修改#include "backward.hpp"为 #include “code_utils/backward.hpp”，再编译。

• 从github上下载imu_utils工具，放在ROS工作空间src中编译

• 将贺博给的ROS版本的imu数据仿真代码放入ROS工作空间src中，将代码中的数据包保存地址更改为自己的地址，编译。
• 在 ~/catkin_ws/src/imu_utils/launch/ 下添加新的 launch 文件vio_imu.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= "simu_imu"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/simu_data/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

运行程序执行标定工具

• 运行

rosrun vio_data_simulation vio_data_simulation_node

生成仿真数据了bag包
运行标定工具imu_utils

roslaunch imu_utils vio_imu.launch

在另一个终端中回放数据包

rosbag play -r 500 ~/data/mydata/imu.bag

其中-r 500表示是以500倍的速率播放这120分钟的数据,一般接近播放完毕时终端2上就
会显示结果和生成数据.~/catkin_ws/src/imu_utils/simu_data/ 目录下, 生成分析数据的文件。

• 标定计算结果保存在~/catkin_ws/src/imu_utils/simu_data/simu_imu_imu_param.yaml中，文件中记录了标定的白噪声的方差和bais的随机游走的值

%YAML:1.0
---
type: IMU
name: simu_imu
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.1260557844682415e-01
      gyr_w: 8.8230495082506407e-04
   x-axis:
      gyr_n: 2.1293785971602505e-01
      gyr_w: 8.0187232882640797e-04
   y-axis:
      gyr_n: 2.1149687710914508e-01
      gyr_w: 1.0019596737493310e-03
   z-axis:
      gyr_n: 2.1338199851530232e-01
      gyr_w: 8.4308284989945300e-04
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6777828551495636e-01
      acc_w: 3.4820781452753759e-03
   x-axis:
      acc_n: 2.6476231602242339e-01
      acc_w: 3.7535073559244805e-03
   y-axis:
      acc_n: 2.6842390901446134e-01
      acc_w: 3.6497517823555676e-03
   z-axis:
      acc_n: 2.7014863150798424e-01
      acc_w: 3.0429752975460792e-03

结果分析

计算机中的运算结果是离散的，要进行连续时间与离散之间的转换运算。imu_utils标定结果的单位如下:

Parameter	YAML element	Symbol	Units
Gyroscope “white noise”	gyr_n
Accelerometer “white noise”	acc_n
Gyroscope “bias Instability”	gyr_w
Accelerometer “bias Instability”	acc_w

设白噪声方差连续时间的仿真参数为$\sigma$，离散采样的标定结果${\sigma }_d$，课上贺博给推的高斯白噪声的连续时间到离散时间之间差一个$\frac{1}{\sqrt{\delta t}}$，$\sqrt{\delta t}$是传感器的采样时间，此时数据仿真设置的采样频率为200Hz，即$\delta t=\frac{1}{200}$，仿真参数$\sigma$与离散采样的标${\sigma }_d$的转换关系如下：
$${\sigma }_d=\frac{\sigma }{\sqrt{\delta t}}$$

同理bias随机游走的噪声方差从连续时间到离散之间需要乘$\sqrt{\delta t}$：
$${\sigma }_{bd}={\sigma }_b\sqrt{\delta t}$$
将标定结果转换到连续时间：
$$gyro.noise.sigma={\sigma }_d\sqrt{\delta t}=2.1260557844682415e-01\ast \frac{1}{\sqrt{200}}=0.015033484623783785$$
$$acc.noise.sigma={\sigma }_d\sqrt{\delta t}=2.6777828551495636e-01\ast \frac{1}{\sqrt{200}}=0.01893478415421331$$
$$gyro.bias.sigma=\frac{{\sigma }_{bd}}{\sqrt{\delta t}}=8.8230495082506407e-04\ast \sqrt{200}=0.012477676276057323$$
$$acc.bias.sigma=\frac{{\sigma }_{bd}}{\sqrt{\delta t}}=3.4820781452753759e-03\ast \sqrt{200}=0.049244021382913894$$

仿真设置的数据为：

   double gyro_noise_sigma = 0.015;    // rad/s
   double acc_noise_sigma  = 0.019;    //m/(s^2)

   double gyro_bias_sigma  = 0.00005;
   double acc_bias_sigma   = 0.0005;

对比设置数据与计算结果，与贺博课上讲的一致，imu_utils工具标定计算白噪声方差计算的还是很准的，但是计算随机游走噪声误差计算的不是很准，建议使用kalibra工具进行imu的标定。

改变仿真参数，重复进行实验

仿真设置的数据为：

   double gyro_noise_sigma = 0.035;    // rad/s
   double acc_noise_sigma  = 0.027;    //m/(s^2)

   double gyro_bias_sigma  = 0.00007;
   double acc_bias_sigma   = 0.0003;

标定计算结果：

%YAML:1.0
---
type: IMU
name: simu_imu
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 4.9375046740011380e-01
      gyr_w: 1.2563330532255593e-03
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 3.8307327060232765e-01
      acc_w: 3.0346208560612866e-03

将标定结果换算到连续时间：
$$gyro.noise.sigma={\sigma }_d\sqrt{\delta t}=4.9375046740011380e-01\ast \frac{1}{\sqrt{200}}=0.034913430371264785$$
$$acc.noise.sigma={\sigma }_d\sqrt{\delta t}=3.8307327060232765e-01\ast \frac{1}{\sqrt{200}}=0.02708737073342152$$
$$gyro.bias.sigma=\frac{{\sigma }_{bd}}{\sqrt{\delta t}}=1.2563330532255593e-03\ast \sqrt{200}=0.017767232427291856$$
$$acc.bias.sigma=\frac{{\sigma }_{bd}}{\sqrt{\delta t}}=3.0346208560612866e-03\ast \sqrt{200}=0.04291601971302123$$

实验结果与第一次一样，白噪声方差计算相对准确，但bias随机游走的方差计算误差很大

绘制allan方差图像，根据图像读数，读出想要的标定值

目前还没有安装MATLAB，安装好之后再来补充allan图像

2. kalibr_allan工具

不在这里专门写kalibr_allan工具了，下一篇专门出一篇记录kalibr工具的blog，不只有allan方差曲线标定imu，还有相机以及相机与imu的联合标定。

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

欧拉积分代码

//读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，
//用来验证数据以及模型的有效性。
void IMU::testImu(std::string src, std::string dist)
{
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);

    double dt = param_.imu_timestep;
    Eigen::Vector3d Pwb = init_twb_;              // position :    from  imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond:  from imu measurements
    Eigen::Vector3d Vw = init_velocity_;          // velocity  :   from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];
        MotionData imuposeK = imudata[i-1]; //贺博的代码i是从1开始的，说明之前的欧拉积分用的是[i+1]时刻的值计算的，所以计算中值积分时用i和i-1时刻的值计算
        //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        Eigen::Quaterniond dq;
        Eigen::Vector3d omega = (imupose.imu_gyro + imuposeK.imu_gyro) / 2.0;   // omegaw
        Eigen::Vector3d dtheta_half =  omega * dt /2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        /// 中值积分
        Eigen::Vector3d acc_w = (Qwb * (imupose.imu_acc) + gw + Qwb * dq * (imuposeK.imu_acc) + gw)/2;  // aw 
        Qwb = Qwb * dq;
        Vw = Vw + acc_w * dt;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;                
        //　按着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;

    }

    std::cout<<"test　end"<<std::endl;

}

imu数据计算好生成后，用准备好的Python工具绘制轨迹图，如下：

欧拉积分计算的轨迹图:


中值积分计算的轨迹图:

可以明显看出,中值积分的效果好于欧拉积分。

3、阅读从已有轨迹生成imu数据的论文, 撰写总结推导:

• 2013年 BMVC, Steven Lovegrove, Spline Fusion: A continuous-timerepresentation for
  visual-inertial fusion withapplication to rolling shutter cameras.

有空再看吧哈哈哈哈