从零手写VIO（二）

程序员文章站 2022-04-16 23:50:14

...

在这里对从零手写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欧拉积分

从零手写VIO（二）
代码：

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

效果：
从零手写VIO（二）

1.5.2中值积分

从零手写VIO（二）
代码：

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

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

2ROS版的IMU仿真代码

2.1 下载编译

下载编译：

mkdir 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 安装im_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 <fstream>

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

3.1.2 标定

采集IMU数据
这里的IMU数据就使用我们上面生成的imu.bag。当然作者也提供了几个包，可以先测试一下。

可以看到，我们之前生成的img.bag，topic为imu, 大概大概四个小时的时长。
写launch文件
可以参考launch文件夹下作者写的几个launch文件，根据自己imu的topic和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= "mems"/>
        <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_topic所对应的value：/imu，imu_name所对应的value：mems，另外要记得把launch文件名改成imu_name，我这里就是mems.launch。

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

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

从零手写VIO（二）
新打开一个terminal窗口，以200倍速播放imu.bag

rosbag play -r 200 imu.bag

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

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

从零手写VIO（二）
以及一个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

绘制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误差。

上一篇： Java学习Java入门

下一篇：超详细的VsCode创建SpringBoot项目(图文并茂)

从零手写VIO（二）

1仿真代码解析

1.1编译vio_data_simulation-master

1.2仿真思路

1.3运动模型

1.4添加噪声

1.5运动模型的离散积分

1.5.1欧拉积分

1.5.2中值积分

2ROS版的IMU仿真代码

2.1 下载编译

2.2 生成imu.bag

3使用Allan方差工具标定

3.1 使用imu_utils标定

3.1.1 安装im_utils

3.1.2 标定

从零学Python之入门（二）基本数据类型

从零学python系列之数据处理编程实例（二）

从零学习node.js之搭建http服务器（二）

从零搭建基于SpringBoot的秒杀系统(二)：快速搭建一个SpringBoot项目

手写VIO第一讲习题:第二题, 更新方式验证

【从零开始手写 VIO】习题

C++从零实现深度神经网络（二）前向传播和反向传播

从零自学Andriod（二）：BottomNavigationView的应用与问题

从零实现一个简易的jQuery框架之二—核心思路详解

从零学大数据系列之数据库篇---第二章:MySQL高级