VINS跑TUM的Visual-Inertial Dataset数据集并使用evo评估
介绍：Visual-Inertial Dataset是TUM在2018年发布的数据集，由实验人员手持设备进行拍摄，用于视觉惯性里程计的评估，提供了标定序列和数据集序列，而且同一序列提供了不同分辨率，分布为512x512和1024x1024，有图片文件夹(tar压缩格式)和适合ros的bag格式。
官方下载及说明网址为https://vision.in.tum.de/data/datasets/visual-inertial-dataset
论文：https://vision.in.tum.de/_media/spezial/bib/schubert2018vidataset.pdf
分为DATASET SEQUENCE(数据集序列)，CALIBRATION (标定序列)，RAW DATA(原始数据)。
使用的传感器设备型号：
标定数据总共提供四种：
1.calib-cam1~8：相机标定，本质矩阵以及两个相机之间的姿态变换
2.calib-imu1~4：IMU标定，找到相机和IMU之间的最优姿态变换
3.calib-vignette2~3：光晕标定？？
4.calib-imu-static2:IMU标定，只包含IMU数据，用于估计IMU噪声和随机游走。格式timestamp, gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z, temperature
数据集序列总共5种：
1.corridor1~5(走廊)：在走廊和几个办公室拍摄，只在开始和结束有真实值。
2.magistrale1~6（大厅）：在校园内的一个大厅拍摄，只在开始和结束有真实值。
3.outdoors1~8(户外)：在校园内的室外场景拍摄，只在开始和结束有真实值。
4.room1~6(室内)：对应有5个图像序列，整个过程都带有运动捕捉系统提高的真实值。
5.slides1~3(滑梯)：在大厅中拍摄，包含一段光照极差的路程，只在开始和结束有真实值。
真实值：由运动捕捉系统OptiTrack Flex13提供，并使用中值滤波去除了外点，以IMU帧形式提供。
VINS:VINS中提供了Visual-Inertial Dataset对应config文件和launch文件，推荐下在room1~6进行评估，因为这些序列中提供了整个过程的真实值，其他数据集序列只提供了开始和结尾的真实值。
EVO：下载完数据集后需要使用EVO进行评估，直接通过pip3安装evo即可。
评估：我评估了带有回环检测的VINS的结果，主要需要修改三个地方。
1.config文件
想要得到带有回环检测的结果需要修改tum_config.yaml文件，首先修改输出路径，有两个地方可以改成同一个路径，第6行的output_path修改为自己指定的路径，然后第70行pose_graph_save_path修改为指定路径，然后开启回环检测，第67行loop_closure改为1，这样才会输出带回环检测的结果。
2.VINS输出结果修改为tum格式
直接输出的结果格式并不能用EVO工具进行评估，需要修改代码,将其输出格式改为tum格式：
首先修改VINS-Mono-master\vins_estimator\src\utility\中的visualization.cpp，从156行开始修改为以下形式：
ofstream foutC(VINS_RESULT_PATH, ios::app); foutC.setf(ios::fixed, ios::floatfield); foutC.precision(0); foutC << header.stamp.toSec() << " "; foutC.precision(5); foutC << estimator.Ps[WINDOW_SIZE].x() << " " << estimator.Ps[WINDOW_SIZE].y() << " " << estimator.Ps[WINDOW_SIZE].z() << " " << tmp_Q.x() << " " << tmp_Q.y() << " " << tmp_Q.z() << " " << tmp_Q.w() << endl;
然后修改VINS-Mono-master\pose_graph\src中的pose_graph.cpp中的addKeyFrame函数，在153行，修改为以下形式：
ofstream loop_path_file(VINS_RESULT_PATH, ios::app); loop_path_file.setf(ios::fixed, ios::floatfield); loop_path_file.precision(0); loop_path_file << cur_kf->time_stamp << " "; loop_path_file.precision(5); loop_path_file << P.x() << " " << P.y() << " " << P.z() << " " << Q.x() << " " << Q.y() << " " << Q.z() << " " << Q.w() << endl;
还是在VINS-Mono-master\pose_graph\src中的pose_graph.cpp中，updatePath函数中的内容也要修改，在630行。
ofstream loop_path_file(VINS_RESULT_PATH, ios::app); loop_path_file.setf(ios::fixed, ios::floatfield); loop_path_file.precision(0); loop_path_file << (*it)->time_stamp << " "; loop_path_file.precision(5); loop_path_file << P.x() << " " << P.y() << " " << P.z() << " " << Q.x() << " " << Q.y() << " " << Q.z() << " " << Q.w() << endl;
最后修改VINS-Mono-master\pose_graph\src中的pose_graph_node.cpp中的main函数，506行，将csv格式改为txt格式：
VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";
到此，VINS输出结果的格式更改完成，接下去修改真实值的格式。
3.真实值修改为tum格式
真实值为对应的.tar文件中的gt_imu.csv文件，由于EVO并不支持这种格式，需要先进行格式转换，转换成.tum文件再和VINS运行的结果进行比较。
转换的命令为：
evo_traj euroc gt_imu.csv --save_as_tum
tum文件格式为timestamp x y z q_x q_y q_z q_w，其中timestamp单位为秒，q是四元数表示。
euroc文件格式为timestamp，px,py,pz,qw,qx,qy,qz,vx,vy,vz,bwx,bwy,bwz,bax,bay,baz 用逗号隔开，其中timestamp单位是纳秒，bw为陀螺仪偏置，ba为加速度计偏置。
得到的gt_imu.tum就是可以使用EVO和VINS结果进行比较的格式，我将其重命名为gt_room1.tum，对应room1序列的真实值。
VINS运行：在catkin_ws目录下打开四个终端，然后分别输入以下命令，终端4的命令用来选择不同数据集。
终端1：
source ~/catkin_ws/devel/setup.bash
roscore
终端2：
source ~/catkin_ws/devel/setup.bash
roslaunch vins_estimator tum.launch
终端3：
source ~/catkin_ws/devel/setup.bash
roslaunch vins_estimator vins_rviz.launch
终端4：
source ~/catkin_ws/devel/setup.bash
rosbag play ~/catkin_ws/Dataset/dataset-room1_512_16.bag
运行完后会生成vins_result_loop.txt，然后使用以下命令进行评估APE或者RPE：
evo_ape tum gt_room1.tum vins_result_loop.txt -va --plot
evo_rpe tum gt_room1.tum vins_result_loop.txt -r angle_deg --delta_unit m -va --plot --plot_mode xyz
得到的结果(仅展示终端部分)：
APE：
Loaded 16541 stamps and poses from: gt_room1.tum Loaded 1358 stamps and poses from: vins_result_loop.txt -------------------------------------------------------------------------------- Synchronizing trajectories... Found 1308 of max. 1358 possible matching timestamps between... gt_room1.tum and: vins_result_loop.txt ..with max. time diff.: 0.01 (s) and time offset: 0.0 (s). -------------------------------------------------------------------------------- Aligning using Umeyama's method... Rotation of alignment: [[ 0.99253067 0.12145606 0.01145834] [-0.12136259 0.99257159 -0.00853021] [-0.01240927 0.00707588 0.99989797]] Translation of alignment: [ 0.85930872 -0.34767327 1.33849877] Scale correction: 1.0 -------------------------------------------------------------------------------- Compared 1308 absolute pose pairs. Calculating APE for translation part pose relation... -------------------------------------------------------------------------------- APE w.r.t. translation part (m) (with SE(3) Umeyama alignment) max 1.261789 mean 0.308258 median 0.262709 min 0.012484 rmse 0.359546 sse 169.089307 std 0.185068
RPE：
Loaded 16541 stamps and poses from: gt_room1.tum Loaded 1358 stamps and poses from: vins_result_loop.txt -------------------------------------------------------------------------------- Synchronizing trajectories... Found 1308 of max. 1358 possible matching timestamps between... gt_room1.tum and: vins_result_loop.txt ..with max. time diff.: 0.01 (s) and time offset: 0.0 (s). -------------------------------------------------------------------------------- Aligning using Umeyama's method... Rotation of alignment: [[ 0.99253067 0.12145606 0.01145834] [-0.12136259 0.99257159 -0.00853021] [-0.01240927 0.00707588 0.99989797]] Translation of alignment: [ 0.85930872 -0.34767327 1.33849877] Scale correction: 1.0 -------------------------------------------------------------------------------- Found 133 pairs with delta 1 (m) among 1308 poses using consecutive pairs. Compared 133 relative pose pairs, delta = 1 (m) with consecutive pairs. Calculating RPE for rotation angle in degrees pose relation... -------------------------------------------------------------------------------- RPE w.r.t. rotation angle in degrees (deg) for delta = 1 (m) using consecutive pairs (with SE(3) Umeyama alignment) max 102.691369 mean 32.609766 median 27.259453 min 0.397103 rmse 41.301337 sse 226871.461814 std 25.345682
我还测试了room2~5的结果，这里不再展示，这个数据集没有提供不同序列之间的差异（比如说速度、光照、纹理情况等，euroc提供了），以下是我自己的主观感觉。
room1速度较快，视角变化剧烈 光照变化明显
room2速度较快，视角变换一般 光照良好
room3 速度极快，视角变换一般
room4 光照和视角变换明显，速度较慢
room5 速度快, 视角变换剧烈
room6 速度慢，视角变换一般
主要参考的文档有以下：
The TUM VI Benchmark for Evaluating Visual-Inertial Odometry论文部分翻译：https://blog.csdn.net/qq_41839222/article/details/86180964
tumVI数据集简单介绍：https://blog.csdn.net/weixin_39926594/article/details/105429925
vins格式修改：https://blog.csdn.net/Hanghang_/article/details/104535370
EVO相关,以及tum,euroc格式：https://github.com/MichaelGrupp/evo/wiki/Formats
