VINS标定---Ego-planner

1、检查realsense 和飞控的连接
查看飞控串口

ls /dev/ttyA*
sodo chmod 777 /dev/ttyACM0
roslaunch mavros px4.launch

查看话题频率,正常频率200Hz

rostopic list
rostopic hz /mavros/imu/data_raw

驱动realsense相机查看图像

roslaunch realsense2_camera ra_camera.launch
rqt_image_view

2、VINS内参参数设置
进入realflight_models下的vins-fusion的config，打开left.yaml修改内参

roslaunch realsense2_camera ra_camera.launch
rostopic list
rostopic echo /camera/infral/camera_info

内参参数顺序 fx cx fy cy
左右目相机参数相同
3、VINS外参参数设置
进入VINS主参数文件stereo_imu_config.yaml 修改外参
output_path对应标定后的vins外参输出，需要手动创建vins_output文件夹

齐次变换矩阵body_T_cam0参数对应旋转矩阵和相机相对于imu的空间位置。旋转矩阵不需要修改，手动测量相机位置填进去后运行vins自动标定
运行vins外参标定脚本

sh shfiles/rspx4.sh
rostopic list 
rostopic echo /vins_fusion/imu_propagate

打开脚本等待大约20秒，VINS正常运行后使用rostopic list找到vins_fusion/imu_propagate
静止状态下position 的x y z 都接近0，拿起飞机绕场地缓慢行走后，如果外参不准确，xyz位置会出现漂移。进入output文件下extrinsic_parameter.csv把运行优化出来的外参替换掉stereo_imu_config.yaml 中手动标定的

一直重复这个过程，直到VINS收敛到满意的程度