手写VIO学习总结（二）

系列笔记:
手写VIO学习总结（一）

1. 作业1

1.1 方法1

• 1 利用vio_data_simulation-ros_version生成imu的静止时候的仿真数据:
  具体步骤:

1.建立work space
2.利用caktin_make编译vio_data_simulation-ros_version
3.rosrun  vio_data_simulation vio_data_simulation_node

注意:1.需要修改vio_data_simulation-ros_version/src/gener_alldata.cpp中的bag路径
2.打开vio_data_simulation-ros_version/src/param.h可以设置参数

• 2 利用高温良的开源代码完成,参考我以前的博文:无人驾驶算法学习（十一）:IMU标定及Allan方差分析
• 3 待续，具体分析

2. 作业2

readme中提示: 1. 编译vio_data_simulation-master 2. 执行bin/data_gen, 生成数据 3. 执行python_tools/draw_trajectory.py 画出轨迹 4. 换成中值积分, 再重做一遍上述1,2,3过程

• 1 欧拉法
  编译vio_data_simulation-master, vio_data_simulation-master是一个Cmake工程,对其进行编译。

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绘图。

说明:
imu_pose.txt是由给定的轨迹方程和欧拉角，生成IMU的pose，imu_int_pose.txt是由给定的轨迹得到速度v和加速度a，再根据欧拉法得到IMU的pose。

cd ../python_tool
python draw_trajctory.py

• 2 中值法
  • 2.1 vio_data_simulation-master/src/imu.cpp中欧拉法

 /// imu 动力学模型 欧拉积分
   Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
   Qwb = Qwb * dq;
   Vw = Vw + acc_w * dt;
   Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

• 2.2 中值法

替换的代码:

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

        //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half = 1.0/2.0* (imupose.imu_gyro +  imupose_.imu_gyro) * dt /2.0;
        // Eigen::Vector3d dtheta_half = (imupose.imu_gyro +  imudata[i-1].imu_gyro)/2 * 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 * (imupose.imu_acc) + gw )/2;  // aw = Rwb * ( acc_body - acc_bias ) + gw
        Qwb = Qwb * dq;
        Vw = Vw + acc_w * dt;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

• 3 注意:
  对比发现中值法更精准!!!