欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

手写VIO作业总结(二)

程序员文章站 2022-04-16 21:21:15
...

作业题目:
手写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过程

欧拉积分得到的仿真图像
手写VIO作业总结(二)
中值积分得到的图像
手写VIO作业总结(二)
具体代码实现

 /************************************欧拉积分*******************************************/
       /*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、提升作业