vins_fusion运行全流程笔记（包含kalibr外参标定笔记）

一、 运行笔记

1.1 VINS

1.1.1 播放bag

需要录制双目压缩图像信息（如果不录压缩图像话题，而是raw话题，那硬盘空间肯定是受不住，三四十个G很正常），imu信息。
-s参数是指bag从哪一秒开始播放。

//播放我自己采的包
rosbag play ~/bag/hgd_200/L_fly.bag -s 328

rosbag play ./real_bag/L_200_playground_1.bag -s 220
rosbag play ./real_bag/L_200_playground_2.bag -s 100
rosbag play 20_meter.bag -s 170
rosbag play golf_course_20_north.bag -s 40
rosbag play golf_course_200_north.bag  -s 75

//记录所有vins_fusion的rviz话题
rosbag record  /cam0/image_raw/compressed /cam1/image_raw/compressed /feature_tracker/feature /globalEstimator/global_path  /loop_fusion/base_path  /loop_fusion/camera_pose_visual /loop_fusion/margin_cloud_loop_rect /loop_fusion/match_image /loop_fusion/odometry_rect /loop_fusion/path_1 /loop_fusion/path_2 /loop_fusion/point_cloud_loop_rect /loop_fusion/pose_graph /loop_fusion/pose_graph_path /mavros/imu/data /mavros/imu/data_raw /mavros/local_position/odom /mavros/local_position/pose /move_base_simple/goal /usb_cam/image_raw/compressed /vins_estimator/camera_pose /vins_estimator/camera_pose_visual /vins_estimator/extrinsic /vins_estimator/image_track /vins_estimator/imu_propagate /vins_estimator/key_poses /vins_estimator/keyframe_point /vins_estimator/keyframe_pose /vins_estimator/margin_cloud /vins_estimator/odometry /vins_estimator/path /vins_estimator/point_cloud /vins_imu_switch /vins_restart /globalEstimator/global_odometry /vins_fusion/odom /rtk/odometry 

1.1.2 图像compressed转raw

由于录制的压缩信息，所以在跑包时需要转成raw才能跑起来，上面那个是ros自带的功能包，后面的参数in 和out就是一样的话题，没有写错。下面的是我自己的写的，图像的时间戳保持原来的时间戳。

//ros自带的，不会时间保持
rosrun  image_transport republish compressed in:=/usb_cam/image_raw  raw  out:=/usb_cam/image_raw
rosrun  image_transport republish compressed in:=/cam0/image_raw  raw  out:=/cam0/image_raw
rosrun  image_transport republish compressed in:=/cam1/image_raw  raw  out:=/cam1/image_raw

//自己写的，有时间保持
roslaunch odom_fusion image_trans.launch
roslaunch odom_fusion image_trans1.launch
roslaunch odom_fusion image_trans2.launch

1.1.3 vins-fusion运行代码

启动三个程序，可视化、vins、回环检测程序。需要制定yaml文件，yaml文件中最主要的参数是外参内参，还有特征点的数量和最小距离。

//可视化rviz
roslaunch vins vins_rviz.launch

//单目加imu
rosrun vins vins_node ~/vins_fusion/src/VINS-Fusion/config/euroc/hgd_200_mono_imu.yaml 
rosrun loop_fusion loop_fusion_node ~/vins_fusion/src/VINS-Fusion/config/euroc/hgd_200_mono_imu.yaml 

//双目
rosrun vins vins_node ~/vins_fusion/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml 
rosrun loop_fusion loop_fusion_node ~/vins_fusion/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml 

//下面是不同的bag对应的参数
//双目加imu
rosrun vins vins_node /home/luli/vins_fusion/src/VINS-Fusion/config/hgd_yaml/golf_course_200_north.yaml 
rosrun loop_fusion loop_fusion_node /home/luli/vins_fusion/src/VINS-Fusion/config/hgd_yaml/golf_course_200_north.yaml 

//双目加imu
rosrun vins vins_node /home/luli/vins_fusion/src/VINS-Fusion/config/hgd_yaml/playground_200_compress.yaml 
rosrun loop_fusion loop_fusion_node /home/luli/vins_fusion/src/VINS-Fusion/config/hgd_yaml/playground_200_compress.yaml 

注意看rviz中相机视角的位姿是否大致正确。

那个相机的红框，跑起来之后应该与你的真实相机位姿差不多，rviz在像素平面的原点即像素左上角有个小方块，方便判断相机的正反。如果与实际差很多，请看本文最后一部分的kalibr外参标定。

1.2 计算VINS协方差，画出误差图

这是我自己写的协方差计算用来和其他值融合，以及和真值误差的可视化。

//计算协方差，以及起飞对齐（在真值2米时，将vins转换到ENU系）
roslaunch odom_fusion covariance.launch

//画误差图
roslaunch odom_fusion plot.laun

1.3 VINS、景象匹配融合

参考的vins_fusion的GPS融合，直接拿来用，哈哈哈，改了一点数据格式啥的。

//vins和景象匹配定位
rosrun global_fusion global_fusion_node

1.4 ros包话题转excel

习惯到matlab里面画图，话题转成excel：

cd ~/vins_fusion/src/odom_fusion/script/
python bag_to_excel_only_one.py

二、 知识点笔记

2.1 kalibr标外参及结果解释

标定外参步骤：kalibr标定双目相机内参与外参
这是我标的结果，重投影误差很大，10个像素点，哈哈哈。猜测可能是相机分辨率比较低，而我用的是aprilgrid标定板，二维码识别效果差，所以误差很大，用棋盘格效果就好很多，误差一个像素。

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[-0.02883653 -0.99790113 -0.05798093  0.05755763]
 [-0.301973    0.06399167 -0.95116632 -0.03714513]
 [ 0.95288024 -0.00991966 -0.3031845  -0.22237409]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[-0.02883653 -0.301973    0.95288024  0.20233882]
 [-0.99790113  0.06399167 -0.00991966  0.05760793]
 [-0.05798093 -0.95116632 -0.3031845  -0.09941433]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.013818300934213368

Transformation (cam1):
-----------------------
T_ci:  (imu0 to cam1): 
[[-0.02889931 -0.99790325 -0.05791313 -0.03962209]
 [-0.30106339  0.06393651 -0.95145833 -0.03859493]
 [ 0.95316613 -0.01006097 -0.30227986 -0.23715885]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam1 to imu0): 
[[-0.02889931 -0.30106339  0.95316613  0.21328721]
 [-0.99790325  0.06393651 -0.01006097 -0.03945743]
 [-0.05791313 -0.95145833 -0.30227986 -0.11070445]
 [ 0.          0.          0.          1.        ]]

timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
-0.01347454249999452

重点：这还不是最大的弯路，之前看到这个第一个外参矩阵的注释（T_ci: (imu0 to cam0) ），我以为是body到cam的外参矩阵，但是实际跑出来不是。
这里T_ci与T_ic分别是指cam到imu的外参矩阵和imu到cam的外参矩阵，这里的注释：T_ci: (imu0 to cam0):和T_ic: (cam0 to imu0):，我感觉是反的，大家使用的时候如果发现外参不对，那俩个外参矩阵都试试。并且因为两个坐标是先旋转再平移的，故而平移矩阵不一样。

2.1.1 坐标系定义

这里比较容易昏，所以我画出相机和imu的坐标系免得后面我遗忘了。

2.1.1.1 PX4飞控IMU坐标系

一般px4的飞控imu是FLU（前、左、上）坐标系，如图：

如果想自己测imu的坐标系，那么可以输出_/mavros/imu/data_，也可以打开rviz可视化查看imu的方向与大小，但是记得把参考坐标系选择为base_link，测试之前必须要知道的是px4imu加速度测量的是支撑力N对应的加速度大小，例如现在我把imu放桌子上，支撑力向上，箭头向上。测试时可以把imu的pitch和roll转个90度，看看x、y轴的正负，就可以判断出来。
静止的飞控支撑力永远竖直向上，飞控的头朝下，x轴加速度为负说明，x轴正方向朝头。

2.1.1.2 相机坐标系

为了和像素平面对应，x轴对应u轴，y轴对应v轴，然后z轴朝前，这是部分资料的定义方式，在kalibr中定义如图，当然也有其他定义方式，比如t265使用了AR/VR中的定义方式就和我们不一样。

2.1.1.3 外参矩阵验证

用工具标定出来，还是自己验证一下比较放心。

T_ic:  (cam0 to imu0): 
[[-0.02883653 -0.301973    0.95288024  0.20233882]
 [-0.99790113  0.06399167 -0.00991966  0.05760793]
 [-0.05798093 -0.95116632 -0.3031845  -0.09941433]
 [ 0.          0.          0.          1.        ]]

外参如上，用一个在线的旋转矩阵转欧拉角网站：3D Rotation Converter

我主要用的就是这几个红框中的数，其他的我没研究过，也看不懂。
输入旋转矩阵，可以看到欧拉角，方便查看可以选成角度模式，一个旋转变换对应一组旋转矩阵、六组欧拉角、一组四元数。欧拉角因为定义的旋转顺序不同导致有六组数据，但是这六组数据都表示的同一变换，所以自己验证的时候需要根据上面的轴顺序来旋转。
同时强调，旋转方向：轴正方向朝向自己，逆时针为正，顺时针为负。