手写VIO作业总结（二）

作业题目：

1、基础作业

1.1 生成标定曲线

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

注意:
1、在启动ros节点时需要把vio_data_simulation-ros_version修改成vio_data_simulation
2、需要修改vio_data_simulation-ros_version/src/gener_alldata.cpp中的bag路径
3、打开vio_data_simulation-ros_version/src/param.h可以设置参数

对ROS: 使用 imu_utils

1. ros下编译 
2. 执行, 生成 imu.bag 
3. rosbag play -r 500 imgimu_utils.bag 回放
4. 用imu_utils进行接收和分析
5. 用imu_utils下的scripts/下的matlab 脚本画allan曲线

对ROS: 使用 kalibr_allan

1. ros下编译 
2. 执行, 生成 imu.bag 
3. 用kalibr_allan的bagconver把bag转成 mat文件, 见readme
4. 用kalibr_allan的m脚本对mat文件进行分析, (需修改m文件中的mat文件路径)
5. 用kalibr_allan的m脚本画allan曲线, (需修改m文件中的result文件路径)

1.2 将imu仿真代码中的欧拉积分替换成中值积分

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

欧拉积分得到的仿真图像

中值积分得到的图像

具体代码实现

 /************************************欧拉积分*******************************************/
       /*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;
        Vw = Vw + acc_w * dt;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;*/
       /***************************************************************************************/


        /*********************************中值积分*********************************************/
        MotionData imupose_pre=imudata[i-1];
        MotionData imupose=imudata[i];
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half=0.5*(imupose_pre.imu_gyro+imupose.imu_gyro)*dt*0.5;

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

2、提升作业