手写VIO作业总结(二)
程序员文章站
2022-04-16 21:21:15
...
作业题目:
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、提升作业
上一篇: 我走上了做鸭的不归路(搞笑)
下一篇: MATLAB标定ZED双目摄像头