devbox
知新_RL
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

《视觉SLAM进阶：从零开始手写VIO》第二讲作业-IMU仿真、MU imu_utils标定_视觉slam进阶:从零开始手写vio

作者：知新_RL | 2024-04-13 10:42:29

赞

踩

视觉slam进阶:从零开始手写vio

《视觉SLAM进阶：从零开始手写VIO》第二讲作业-IMU仿真、MU imu_utils标定

作业题目：

在这里插入图片描述

在这里插入图片描述

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

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

因为有脚本文件build.sh，或者

chmod +x build.sh 
./build.sh 
cd ../bin
./data_gen

  • 1
  • 2
  • 3
  • 4

运行完可执行文件data_gen后，会在bin目录下生成以下txt文件

imu_pose_noise.txt ，imu_pose.txt ，imu_int_pose_noise.txt， imu_int_pose.txt

在这里插入图片描述

1.1.1 欧拉积分法

（1）公式

在这里插入图片描述

（2）代码

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

    MotionData imupose = imudata[i];//imu的位姿

    //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]； 即qwb′  = qwb ⊗ [1  1/2 * ω∆t ]
    Eigen::Quaterniond dq;
    // 欧拉积分
    Eigen::Vector3d dtheta_half = imupose.imu_gyro  * dt /2.0; // qwb′  = qwb ⊗ [1  1/2 * ω∆t ] 的 1/2 * ω∆t 部分，ω是角速度的真实值imudata[i].imu_gyro 
    // 中值积分
    // Eigen::Vector3d dtheta_half = ( imudata[i - 1].imu_gyro + imupose.imu_gyro)/ 2.0 * 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;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

（3）轨迹对比图

使用python-tool下的Python脚本，可以绘制出点，IMU轨迹等

cd ../python_tool
python3 draw_trajctory.py

  • 1
  • 2

但是我运行发现报错：python3: can’t open file ‘draw_trajctory.py’: [Errno 2] No such file or directory

原因是：下载的代码是draw_trajcory.py少了字幕t，需要改为draw_trajctory.py。

运行结果如图：

在这里插入图片描述

1.1.2 中值积分法

（1）公式

在这里插入图片描述

（2）代码

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

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

    MotionData imupose = imudata[i];//imu的位姿

    //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]； 即qwb′  = qwb ⊗ [1  1/2 * ω∆t ]
    Eigen::Quaterniond dq;
    // 欧拉积分
    //Eigen::Vector3d dtheta_half = imupose.imu_gyro  * dt /2.0; // qwb′  = qwb ⊗ [1  1/2 * ω∆t ] 的 1/2 * ω∆t 部分，ω是角速度的真实值imudata[i].imu_gyro 
    // 中值积分
    Eigen::Vector3d dtheta_half = ( imudata[i - 1].imu_gyro + imupose.imu_gyro)/ 2.0 * 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;

    // /// imu 动力学模型 中值积分
    Eigen::Vector3d acc_w = (Qwb*(imudata[i - 1].imu_acc) + gw + Qwb*dq*(imupose.imu_acc) + gw) / 2.0;
    Qwb = Qwb * dq;
    Vw = Vw + acc_w * dt;
    Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27

（3）轨迹对比图

./build.sh 
cd ../bin
./data_gen
 cd ../python_tool
python3 draw_trajctory.py 

  • 1
  • 2
  • 3
  • 4
  • 5

在这里插入图片描述

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

简化运行命令：

命令为了方便以后的运行，修改build.sh文件

修改build.sh文件

#!/bin/bash
echo "Configuring and building ..."
if [ ! -d "build" ]; then
    mkdir build
fi
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

增加以下代码：


cd ../bin
./data_gen
cd ../python_tool
python3 draw_trajctory.py

  • 1
  • 2
  • 3
  • 4
  • 5

以后只需要运行./build.sh就可以看见轨迹对比图，或者运行 bash build.sh也可以，且方便cout的调试。

运行draw_points.py代码，可得下图：

（将build.sh的python3 draw_trajctory.py改为python3draw_points.py）

./build.sh

  • 1

在这里插入图片描述

有报错

Traceback (most recent call last):
  File "draw_points.py", line 30, in <module>
    x.append( numbers_float[0] )
TypeError: 'map' object is not subscriptable

  • 1
  • 2
  • 3
  • 4

**解决办法：**使用list包装出现问题的对象，list（），共有4处要修改。

numbers_float = map(float, odom)  # 转化为浮点数
numbers_float = list(numbers_float)

  • 1
  • 2

可以看出IMU是绕房子做Z轴的正弦运动

1.2仿真思路

指定轨迹方程,求一阶导得到速度, 角速度,求二阶导得到加速度。对加速度、角速度添加高斯白噪声和bias 的随机游走噪声，得到仿真数据。

在这里插入图片描述

1.3运动模型

在该仿真代码中，在imu.cpp中定义了IMU的运动模型：

// 指定轨迹方程，求一阶导得到速度,角速度，求二阶导得到加速度
// 并将求得的 速度  角速度 加速度 时间戳 旋转矩阵 平移矩阵 赋值给结构体MotionData对象data的成员变量
MotionData IMU::MotionModel(double t)
{

    MotionData data;//结构体对象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 平移
    // 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);  //t=0 起点 (20,5,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;
    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]
    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   //加速度
//  struct MotionData的成员赋值
    data.imu_gyro = imu_gyro;//陀螺仪   欧拉角速度     IMU的角速度真实值
    data.imu_acc = imu_acc;//加速度计   加速度         IMU的加速度真实值
    data.Rwb = Rwb; //旋转矩阵
    data.twb = position; //平移矩阵
    data.imu_velocity = dp; //速度
    data.timestamp = t;//时间戳
    return data;

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45

1.4添加噪声

设定,加速度的高斯白噪声设定为 0.019, 陀螺仪的高斯白噪声为 0.015. 加速度 bias 的随机游走噪声设定为 5e−4 ,陀螺仪的 bias 随机游走噪声设定为 5e−5 。

在这里插入图片描述

param.h定义了参数

class Param{

public:

    Param();

    // time
    int imu_frequency = 200;
    int cam_frequency = 30;
    double imu_timestep = 1./imu_frequency; // imu的采样时间  0.005s
    double cam_timestep = 1./cam_frequency; // 相机的采样时间  1/30s
    double t_start = 0.;//起始时间
    double t_end = 20;  //  20 s  结束时间

    // noise
    double gyro_bias_sigma = 1.0e-5;//陀螺仪的 bias 随机游走噪声
    double acc_bias_sigma = 0.0001;//加速度 bias 的随机游走噪声

    double gyro_noise_sigma = 0.015;    // rad/s * 1/sqrt(hz)  陀螺仪的高斯白噪声
    double acc_noise_sigma = 0.019;      //　m/(s^2) * 1/sqrt(hz)  加速度的高斯白噪声

    double pixel_noise = 1;              // 1 pixel noise

    // cam f
    double fx = 460;
    double fy = 460;
    double cx = 255;
    double cy = 255;
    double image_w = 640;
    double image_h = 640;


    // 外参数
    Eigen::Matrix3d R_bc;   // cam to body
    Eigen::Vector3d t_bc;     // cam to body

};

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
//向imu数据data里添加离散时间的高斯白噪声noise和离散时间的bias随机游走，并更新 加速度和角速度
void IMU::addIMUnoise(MotionData& data)
{
    std::random_device rd;//非确定性(真)随机数生成器  
    std::default_random_engine generator_(rd());//创建随机数引擎对象  传入一个随机数种子
    std::normal_distribution<double> noise(0.0, 1.0);//随机数的正态连续分布。

    Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值   方差为 1 的白噪声
    // cout <<"noise_gyro:"  << endl;
    // cout<<noise_gyro << endl;
    // noise_gyro:
    // -1.35678
    // -0.33964
    // -0.199193
    // noise_gyro:
    // 0.264621
    // -0.522613
    // 0.0166623
    Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();// 陀螺仪的高斯白噪声协方差
    //     gyro_sqrt_cov:
    // 0.015     0     0
    //     0 0.015     0
    //     0     0 0.015

    // cout <<"gyro_sqrt_cov:"  << endl;
    // cout<<gyro_sqrt_cov << endl;
    // 忽略 scale 的影响，只考虑白噪声和 bias 随机游走
    // 角速度 w = w + 离散时间的高斯白噪声noise + 离散时间的bias随机游走
    // 高斯白噪声的连续时间到离散时间之间差一个 1/√∆t ,√∆t是采样时间 
    // 离散时间的高斯白噪声noise: nd[k] = sigema/√∆t  * w[k] = gyro_sqrt_cov  / sqrt( param_.imu_timestep ) * noise_gyro
    data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;  //3个轴的角速度一起算
    // IMU的角速度测量值 imu_gyro
    // cout <<"data.imu_gyro:"  << endl;
    // cout<<data.imu_gyro << endl;
    // data.imu_gyro:
    // -0.380758
    // 0.118272
    // 0.0782825
    // data.imu_gyro:
    // -0.275656
    // 0.115311
    // 0.618848
    Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值   方差为 1 的白噪声
    Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();// 加速度计的高斯白噪声协方差
    // 忽略 scale 的影响，只考虑白噪声和 bias 随机游走
    // 加速度 a = a + 离散时间的高斯白噪声noise + 离散时间的bias随机游走
    // 离散时间的高斯白噪声noise: nd[k] = sigema/√∆t  * w[k] = acc_sqrt_cov  / sqrt( param_.imu_timestep ) * noise_acc
    // IMU的加速度测量值 imu_acc
    data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;

    // gyro_bias update 陀螺仪 离散时间的bias随机游走 更新
    Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值   方差为 1 的白噪声
    // bias 随机游走的噪声方差从连续时间到离散之间需要乘以 √∆t 
    // 离散时间的bias随机游走gyro_bias_  ：bd[k] = bd[k-1] + sigema * √∆t * w[k] = param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
    gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
    data.imu_gyro_bias = gyro_bias_;// 陀螺仪 离散时间的bias随机游走 更新

    // acc_bias update 加速度计 离散时间的bias随机游走 更新
    Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值   方差为 1 的白噪声
    // bias 随机游走的噪声方差从连续时间到离散之间需要乘以 √∆t 
    // 离散时间的bias随机游走acc_bias_  ：bd[k] = bd[k-1] + sigema * √∆t * w[k] = param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
    acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
    data.imu_acc_bias = acc_bias_;// 加速度计 离散时间的bias随机游走 更新

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65

2 ROS版的IMU仿真代码

2.1 下载编译vio_data_simulation的ros版本

创建catkin工作空间：先创建catkin工作空间，依次输入如下指令。

mkdir -p vio_sim_ws/src
cd vio_sim_ws/src
catkin_init_workspace   //初始化工作空间  

  • 1
  • 2
  • 3

下载vio_data_simulation

终端输入如下指令，git下载vio_data_simulation包：

git clone https://github.com/HeYijia/vio_data_simulation
cd vio_data_simulation            
git checkout ros_version

  • 1
  • 2
  • 3

git checkout ros_version 命令是切换到 ros_version 分支

cd ..
cd ..
catkin_make

  • 1
  • 2
  • 3

在这里插入图片描述

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

我没改，默认的路径是： HOME/cgm/imu.bag

 const std::string home_path = getenv("HOME");
 const std::string bag_path = home_path + "/imu.bag";
 rosbag::Bag bag;
 bag.open(bag_path, rosbag::bagmode::Write);

  • 1
  • 2
  • 3
  • 4

然后，启动节点，生成imu.bag，这个文件有1.1GB。

source devel/setup.bash //设置ros环境
rosrun vio_data_simulation vio_data_simulation_node

  • 1
  • 2

至此，我们得到了一个IMU仿真数据的ROS bag包。接下来利用Allan方差工具对其进行标定。

在这里插入图片描述

ROS省略source devel/setup.bash的方法

为了不每次运行程序的时候都source一次devel文件夹里的setup.bash，可以打开主目录 按下Crtl+h 显示隐藏文件，双击打开bashrc文件，在最后加入
source /home/############/catkin_ws/devel/setup.bash，其中###部分为你的计算机账户，保存后再重新打开终端，显示无错误即可。

在这里插入图片描述

3 使用Allan方差工具标定

主要是使用imu_utils和kalibr_allan进行标定

3.1 使用imu_utils标定

3.1.1 安装code_utils

我们的 code_utils 和 imu_utils 都安装在 imu-calibration/src文件夹下面。

首先，安装依赖：

sudo apt-get install libdw-dev

  • 1

先code_utils，再编译code_utils

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

  • 1
  • 2
  • 3
  • 4
  • 5

在这里插入图片描述

在这里插入图片描述

3.1.2 编译code_utils的报错解决

错误1：找不到Eigen
我们一般通过以下命令安装Eigen：

sudo apt-get install libeigen3-dev

  • 1

**解决办法：**这样Eigen就默认安装在/usr/include/eigen3，需要在/home/cgm/imu-calibration/src/code_utils/CMakeLists.txt中注释掉find_package(Eigen3 REQUIRED)，然后添加：

include_directories(/usr/include/eigen3)

  • 1

错误2：找不到backward.hpp

atal error: backward.hpp: No such file or directory

**解决办法：**把文件/home/cgm/imu-calibration/srccode_utils/src/sumpixel_test.cpp中的#include "backward.hpp"改成#include "code_utils/backward.hpp"即可。

**错误3：**std::ofstream未定义

/home/cgm/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>

  • 1

以下错误4 5 6是由于opencv 版本导致的

**错误4：**error: ‘CV_LOAD_IMAGE_UNCHANGED

/home/cgm/imu-calibration/src/code_utils/src/mat_io_test.cpp:33:47: error: ‘CV_LOAD_IMAGE_UNCHANGED’ was not declared in this scope
33 | Mat img1 = imread( “/home/gao/IMG_1.png”, CV_LOAD_IMAGE_UNCHANGED );

**解决办法：**打开/home/cgm/imu-calibration/src/code_utils/src/mat_io_test.cpp 点击 “搜索”，输入“CV_LOAD_IMAGE_UNCHANGED”，替换成-1

替换成cv::IMREAD_UNCHANGED也可以

表示读取原图

Mat img1 = imread( "/home/gao/IMG_1.png",  CV_LOAD_IMAGE_UNCHANGED);
改为
Mat img1 = imread( "/home/gao/IMG_1.png", -1 );
或者
Mat img1 = imread( "/home/gao/IMG_1.png", cv::IMREAD_UNCHANGED );

  • 1
  • 2
  • 3
  • 4
  • 5

**错误5：**error: ‘CV_LOAD_IMAGE_GRAYSCALE

/home/cgm/imu-calibration/src/code_utils/src/sumpixel_test.cpp:84:47: error: ‘CV_LOAD_IMAGE_GRAYSCALE’ was not declared in this scope
84 | Mat img1 = imread( “/home/gao/IMG_1.png”, CV_LOAD_IMAGE_GRAYSCALE );
| ^~~~~~~~~~~~~~~~~~~~~~~

**解决办法：**打开/home/cgm/imu-calibration/src/code_utils/src/sumpixel_test.cpp 点击 “搜索”，输入“CV_LOAD_IMAGE_GRAYSCALE”，替换成0

替换成cv::IMREAD_GRAYSCALE也可以

表示读取灰度图

原因：OPENCV的API已改

我的opencv是4.4.5版本，查看ubuntu下的OpenCV安装版本：

pkg-config opencv --modversion

  • 1

在这里插入图片描述
改为了
在这里插入图片描述
https://docs.opencv.org/3.4/da/d0a/group__imgcodecs__c.html#gga8420dba71b9cc240cf981b0bef892004a7dc9fd0941c4a96edd4dcbacdc98518c
https://docs.opencv.org/3.4/d8/d6a/group__imgcodecs__flags.html

报错6： error: ‘CV_MINMAX’

/home/cgm/imu-calibration/src/code_utils/src/sumpixel_test.cpp:94:35: error: ‘CV_MINMAX’ was not declared in this scope; did you mean ‘CV_MMX’?
94 | normalize( img, img2, 0, 255, CV_MINMAX );

**解决办法：**把CV_MINMAX改成NORM_MINMAX

3.1.3 安装imu_utils

先下载imu_utils，再编译imu_utils

cd imu-calibration/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

  • 1
  • 2
  • 3
  • 4

至此，code_utils和imu_utils都安装好了。

在这里插入图片描述

ROS省略source devel/setup.bash的方法

将source /home/cgm/imu-calibration/devel/setup.bash 添加到.bashrc文件的末尾

在这里插入图片描述

3.1.3 标定

  • 查看imu.bag的信息

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

    在这里插入图片描述

​ 可以看到，我们之前生成的img.bag，topic为imu（这个信息后文中的.launch文件需要用到。）, 大概四个小时的时长。

  • 写launch文件

    可以参考**/home/cgm/imu-calibration/src/imu_utils/launch**下作者写的几个launch文件，根据自己imu的topic和name进行修改：

在这里插入图片描述

进入如图所示路径，复制16448.launch文件，将文件名改为my.launch，打开 my.launch，将其中的imu_topic与imu_name分别改为imu（前文提到过）和mytest。

在这里插入图片描述

还可一看到data的存储路径是imu_utils/data文件夹里，可以先清空data文件夹，或者新建个文件夹暂时全部移动过去，这样能更清楚地明白生成了哪些文件。

在这里插入图片描述

  • 回放数据包，标定结果

（1）启动roscore

roscore

  • 1

在这里插入图片描述

（2）进入imu-calibration文件夹里，新打开一个terminal窗口，以500倍速播放imu.bag。注意我的imu.bag在 HOME/cgm/imu.bag

rosbag play -r 500 ../imu.bag

  • 1

（3）进入imu-calibration文件夹里，新打开一个terminal窗口，运行我们的my.launch

roslaunch imu_utils my.launch

  • 1

我这里不要再配置环境命令了 source ./devel/setup.bash ，见ROS省略source devel/setup.bash的方法

在这里插入图片描述

在这里插入图片描述

执行结束之后，新生成的17个文件

在这里插入图片描述

（4）打开文件mytest_imu_param.yaml，部分文件内容如下，这就是标定结果。

这里应该格外注意单位rad/s和m/s^2。

%YAML:1.0
---
type: IMU
name: mytest
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.1174628193741066e-01
      gyr_w: 8.5047491324959210e-04
   x-axis:
      gyr_n: 2.1124530533883673e-01
      gyr_w: 7.4517858725206359e-04
   y-axis:
      gyr_n: 2.1327773065877687e-01
      gyr_w: 9.6551201149405936e-04
   z-axis:
      gyr_n: 2.1071580981461840e-01
      gyr_w: 8.4073414100265334e-04
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6979648977867060e-01
      acc_w: 3.8990219990271773e-03
   x-axis:
      acc_n: 2.6807138890223103e-01
      acc_w: 3.9648009709693111e-03
   y-axis:
      acc_n: 2.7120534163617294e-01
      acc_w: 3.7621137218341555e-03
   z-axis:
      acc_n: 2.7011273879760778e-01
      acc_w: 3.9701513042780636e-03

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32

imu_utils默认给出了IMU的白噪声（ “white noise”）和零偏不稳定性（ “bias Instability”）这两个参数的标定结果。

而我们设置的噪声参数以及kalibr_allan标定的结果都是IMU的白噪声（ “white noise”）和传感器Bias的随机游走（Rate Random Walk）。至于如何用imu_utils给出传感器Bias的随机游走（Rate Random Walk）不在这里论述。

在这里插入图片描述

虽然我们得到了标定结果，但这个标定结果并不是我们最终的结果。现在得到的结果的单位是rad/s和m/s^2，而老师param.h代码中给出的单位如下面代码中最后两行，后面多了**/sqrt(hz)**。上面的图Units一栏也能说明。

噪声是个能量概念或者说功率概念，我们还要把标定得到的参数归一化到每单位sqrt(hz)尺度下。

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

int imu_frequency = 200; //IUM的频率是200Hz

double gyro_noise_sigma = 0.015;    // rad/s * 1/sqrt(hz)  陀螺仪的高斯白噪声
double acc_noise_sigma = 0.019;      //　m/(s^2) * 1/sqrt(hz)  加速度的高斯白噪声

  • 1
  • 2
  • 3
  • 4
	unit:  		rad/s						rad/s * 1/sqrt(hz)				给定值
  gyr_n: 2.1174628193741066e-01				0.0149727						0.015
  gyr_w: 8.5047491324959210e-04				6.01377e-05
	
    unit: 		m/s^2						m/(s^2) * 1/sqrt(hz)
  acc_n: 2.6979648977867060e-01				0.0190775						0.019
  acc_w: 3.8990219990271773e-03				0.000275702

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

可以在程序中增加代码进行简单计算验证：

cout << "2.1174628193741066e-01 / sqrt(200) = " << 2.1174628193741066e-01 / sqrt(200) << endl;
cout << "8.5047491324959210e-04 / sqrt(200) = " << 8.5047491324959210e-04 / sqrt(200) << endl;
cout << "2.6979648977867060e-01 / sqrt(200) = " << 2.6979648977867060e-01 / sqrt(200) << endl;
cout << "3.8990219990271773e-03 / sqrt(200) = " << 3.8990219990271773e-03 / sqrt(200) << endl;
结果：
2.1174628193741066e-01 / sqrt(200) = 0.0149727
8.5047491324959210e-04 / sqrt(200) = 6.01377e-05
2.6979648977867060e-01 / sqrt(200) = 0.0190775
3.8990219990271773e-03 / sqrt(200) = 0.000275702

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

3.2 使用MATLAB画Allan方差图

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

windows系统下

3.2.1 生成陀螺仪的Allen方差图

这里以生成陀螺仪的Allen方差图为例，修改scripts中的draw_allan.m代码（所有的A3替换成mytest）：

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 , '-');

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

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

在这里插入图片描述

3.2.2 生成加速度计的Allen方差图

同理，将文件中的所有gyr替换成acc,得到加速度的Allan方差图如下：

clear 
close all

dt = dlmread('../data/data_mytest_acc_t.txt');         
data_x = dlmread('../data/data_mytest_acc_x.txt'); 
data_y= dlmread('../data/data_mytest_acc_y.txt'); 
data_z = dlmread('../data/data_mytest_acc_z.txt'); 
data_draw=[data_x data_y data_z] ;

data_sim_x= dlmread('../data/data_mytest_sim_acc_x.txt'); 
data_sim_y= dlmread('../data/data_mytest_sim_acc_y.txt'); 
data_sim_z= dlmread('../data/data_mytest_sim_acc_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 , '-');

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

在这里插入图片描述

包括前面我们留下的传感器Bias的随机游走（Rate Random Walk）在内的所有IMU参数，都可以通过分析这Allan方差图得到。

具体可以参考论文：UCAM-CL-TR-696

Allan Variance. “Noise Analysis for Gyroscopes”. In: Freescale Semiconductor Document Number: AN5087 Application Note Rev. 0 2 (2015).]:

网址：chrome-extension://ikhdkkncnoglghljlkmcimlnlhkeamad/pdf-viewer/web/viewer.html?file=https%3A%2F%2Ftelesens.co%2Fwp-content%2Fuploads%2F2017%2F05%2FAllanVariance5087-1.pdf#=&zoom=190

例如：其中的19页

在这里插入图片描述

在这里插入图片描述

[5] IEEE Std 962-1997 (R2003) Standard Specification Format Guide and Test Procedure for Single-Axis Interferometric Fiber Optic Gyros, Annex C. IEEE, 2003.

3.2.3 Allan方差图读取误差系数

Allan方差法可用于5种随机误差的标定：

量化噪声(Quantization Noise)：误差系数为Q，Allan方差双对数曲线上斜率为-1的直线延长线与t=10°的交点的纵坐标读数为√3 Q；
角度随机游走(Angle Random Walk)：其误差系数N，Allan方差双对数曲线上斜率为-1/2的线的延长线与t=10°交点的纵坐标读数即为N；
零偏不稳定性(Bias Instability)：其误差系数B，Allan方差双对数曲线上斜率为0的线的延长线与t=10°交点的纵坐标读数为√((2 ln⁡2)/π) B，一般常取底部平坦区的最小值或取t=101或t=102处的值；
角速率随机游走(Rate Random Walk)：其误差系数K，斜率为+1/2的线的延长线与t=10°交点的纵坐标读数为K/√3；
角速率斜坡(Rate Ramp)：其误差系数R，斜率为+1的线的延长线与t=10°交点的纵坐标读数为R/√2；

4 使用kalibr_allan工具标定

因为am=a+na+ba,即测量值=真实值+白噪声+bias

所以需要标定白噪声和bias。

allan方差斜率中用到的是①陀螺仪加速度计的随机游走，对应的斜率是-0.5；②bias的随机游走，对应的斜率是0.5；故allan方差需要是钩子形状，才能成功标定。

建议去看一下 https://github.com/rpng/kalibr_allan.git 的Readme.md文件

4.1 安装kalibr_allan

cd imu-calibration/src
git clone https://github.com/rpng/kalibr_allan.git
cd ..
catkin_make

  • 1
  • 2
  • 3
  • 4

安装Kalibr，在执行catkin_make时出现错误The following variables are used in this project, but they are set to NO

安装Kalibr 的时候，bagconvert设置的默认路径找不到matlab这样导致安装失败出现以下情况：

CMake Error: The following variables are used in this project, but they are set to NOTFOUND.
Please set them or make sure they are set and tested correctly in the CMake files:
MATLAB_ENG_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_INCLUDE_DIR (ADVANCED)
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_MAT_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_MEX_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_MX_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_UT_LIBRARY
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert

– Configuring incomplete, errors occurred!
See also “/home/cgm/imu-calibration/build/CMakeFiles/CMakeOutput.log”.
See also “/home/cgm/imu-calibration/build/CMakeFiles/CMakeError.log”.
make: *** [Makefile:824：cmake_check_build_system] 错误 1
Invoking “make cmake_check_build_system” failed

原因：缺少matlab

4.2 安装matlab

我极度不想安装matlab-_-，就不做了。

后面的参考这个吧
从零手写VIO（二）——IMU仿真、MU imu_utils或kalibr_allan标定

本文内容由网友自发贡献，转载请注明出处：https://www.wpsshop.cn/w/知新_RL/article/detail/415966
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号