Vins-Fusion运行kitti,euroc和tum数据集并使用evo评估

基于ubuntu18.04

VIns-Fusion

1.修改程序输出位姿信息修改为TUM格式

为了方便评估，先将程序的输出位姿信息修改为tum格式，需要做如下改动

1.1 回环输出位姿文件pose_graph.cpp

updatePath()函数中，下面一段修改为

        if (SAVE_LOOP_PATH)
        {
            ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(9);
            loop_path_file << (*it)->time_stamp  << " ";
            loop_path_file.precision(5);    //为进行评估，这里修改 x,y,z,w
            loop_path_file  << P.x() << " "
                  << P.y() << " "
                  << P.z() << " "
                  << Q.x() << " "
                  << Q.y() << " "
                  << Q.z() << " "
                  << Q.w() << endl;
            loop_path_file.close();
        }

addKeyFrame()函数中，下面一段修改为

        if (SAVE_LOOP_PATH)
        {
            ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(9);
            loop_path_file << cur_kf->time_stamp  << " ";
            loop_path_file.precision(5);    //为进行评估，这里修改 x,y,z,w
            loop_path_file  << P.x() << " "
                  << P.y() << " "
                  << P.z() << " "
                  << Q.x() << " "
                  << Q.y() << " "
                  << Q.z() << " "
                  << Q.w() << endl;
            loop_path_file.close();
        }

1.2 里程计位姿输出visualization.cpp

如果还要分析纯里程计的位姿信息，还需要修改visualization.cpp中的pubOdometry()函数,如下修改

 // write result to file
 // ofstream foutC(VINS_RESULT_PATH, ios::app);
 // foutC.setf(ios::fixed, ios::floatfield);
 // foutC.precision(0);
 // foutC << header.stamp.toSec() * 1e9 << ",";
 // foutC.precision(5);
 // foutC << estimator.Ps[WINDOW_SIZE].x() << ","
 //       << estimator.Ps[WINDOW_SIZE].y() << ","
 //       << estimator.Ps[WINDOW_SIZE].z() << ","
 //       << tmp_Q.w() << ","
 //       << tmp_Q.x() << ","
 //       << tmp_Q.y() << ","
 //       << tmp_Q.z() << ","
 //       << estimator.Vs[WINDOW_SIZE].x() << ","
 //       << estimator.Vs[WINDOW_SIZE].y() << ","
 //       << estimator.Vs[WINDOW_SIZE].z() << "," << endl;
 // foutC.close();

 // write result to file   这里写入的是no_loop.csv
 ofstream foutC(VINS_RESULT_PATH, ios::app);
 foutC.setf(ios::fixed, ios::floatfield);
 foutC.precision(5);
 foutC << header.stamp.toSec() << " "<< 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;
 foutC.close();

1.3 修改配置文件的输出路径

修改每个数据集对应的yaml文件中的输出路径，这个看自己需求习惯

output_path: "~/catkin_ws_vinsFusion/src/VINS-Fusion/output/"

以上修改中一定要注意时间戳的精度，对很大影响evo评估的精度

2.安装evo工具

安装

pip install evo --upgrade --no-binary evo

这样就可以使用相关命令

evo_traj tum myslam.txt -p
evo_ape tum data.txt vins_data.txt  -va -p
--save_as_tum         save trajectories in TUM format (as *.tum)
--save_as_kitti          save poses in KITTI format (as *.kitti)
--save_as_bag         save trajectories in ROS bag as <date>.bag
--save_plot “保存的路径”

详细使用可参考https://blog.csdn.net/CSDNhuaong/article/details/101909888

但是为了对数据集中的groundTruth进行处理，最好下载一下evo的源码，其中有些脚本需要使用
git地址https://github.com/MichaelGrupp/evo.git

3.运行kitti_odometry数据集

下载地址http://www.cvlibs.net/datasets/kitti/eval_odometry.php,groundTruth也可以在这个页面下载。

3.1 使用evo工具转换格式

首先把kitti的groundTruth文件转为TUM格式，在下载的evo源代码路径下，进入contrib文件夹，打开终端运行，注意修改为自己的路径

python kitti_poses_and_timestamps_to_trajectory.py kitti路径/data_odometry_gray/dataset/KITTIOdometry_data_odometry_poses/dataset/poses/05.txt kitti路径/data_odometry_gray/dataset/sequences/05/times.txt kitti_05_gt.txt

以上命令结合kitti的时间戳和位姿生成轨迹文件kitti_05_gt.txt，这个文件接下来作为groundTruth与VinsFusion得到的轨迹进行评估。

3.2 运行Vins_Fusion

一下三个命令在对应工作空间的三个终端启动，假设运行kitti05，注意修改为自己的路径
注意每次要source devel/setup.bash

roslaunch vins vins_rviz.launch
rosrun loop_fusion loop_fusion_node ~/catkin_ws_vinsFusion/src/VINS-Fusion/config/kitti_odom/kitti_config04-12.yaml
rosrun vins kitti_odom_test src/VINS-Fusion/config/kitti_odom/kitti_config04-12.yaml kitti路径/data_odometry_gray/dataset/sequences/05

运行结束即可在设定的路径下得到vio_loop.csv

3.3 evo评估

根据上面得到的两个文件，计算轨迹误差

evo_ape tum vio_loop.csv ~/下载/evo/contrib/kitti_05_gt.txt -va --plot --plot_mode xyz
evo_rpe tum vio_loop.csv ~/下载/evo/contrib/kitti_05_gt.txt -r full -va --plot --plot_mode xyz

回环的groundTruth文件可以在https://github.com/ZhangXiwuu/KITTI_GroundTruth下载，不过这里用不到。

4.运行euroc数据集

下载地址https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets

4.1 使用evo工具转换格式

将euroc数据集本身的groundTruth改为TUM格式，在数据集的groundTruth路径下，打开终端运行

evo_traj euroc data.csv --save_as_tum

即可得到data.tum文件，作为groundTruth

4.2 运行Vins_Fusion

以MH_01为例子，注意修改路径以及 source devel/setup.bash,在对应工作空间，每个命令使用一个终端运行

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws_Fine/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
rosrun loop_fusion loop_fusion_node ~/catkin_ws_Fine/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
rosbag play ~/euroc/MH_01_easy.bag

运行结束可以在指定的输出路径下得到vio_loop.csv

4.3 evo评估

根据上面得到的两个文件，计算轨迹误差

evo_ape tum vio_loop.csv ~/euroc/MH_01_easy/mav0/state_groundtruth_estimate0/data.tum -va --plot --plot_mode xyz
evo_rpe tum vio_loop.csv ~/euroc/MH_01_easy/mav0/state_groundtruth_estimate0/data.tum -r full -va --plot --plot_mode xyz

5. 运行TUM VI数据集

下载地址：https://vision.in.tum.de/data/datasets/visual-inertial-dataset

5.1 使用evo工具转换格式

TUM数据集的GroundTruth在对应的tar文件中即~/TUM/dataset-room1_512_16/dso/gt_imu.csv
在这个路径下，打开终端执行

evo_traj euroc gt_imu.csv --save_as_tum

得到gt_imu.tum,作为接下来评估的groundTruth

5.2 运行Vins_Fusion

以room1为例，注意修改路径以及 source devel/setup.bash,在对应工作空间，每个命令使用一个终端运行

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws_vinsFusion/src/VINS-Fusion/config/TUM/tum_mono_imu.yaml
rosrun loop_fusion loop_fusion_node ~/catkin_ws_vinsFusion/src/VINS-Fusion/config/TUM/tum_mono_imu.yaml
rosbag play ~/TUM/dataset-room1_512_16.bag

tum_mono_imu.yaml写法如下（可参考vinsmono的）

%YAML:1.0
 
imu: 1         
num_of_cam: 1  
 
#common parameters
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
output_path: "/home/sch/catkin_ws_vinsFusion/src/VINS-Fusion/output/"
 
cam0_calib: "cam0.yaml"
image_width: 512
image_height: 512
 
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -9.9951465899298464e-01, 7.5842033363785165e-03, -3.0214670573904204e-02, 4.4511917113940799e-02,
            2.9940114644659861e-02, -3.4023430206013172e-02, -9.9897246995704592e-01, -7.3197096234105752e-02,
            -8.6044170750674241e-03, -9.9939225835343004e-01, 3.3779845322755464e-02 ,-4.7972907300764499e-02,
            0,   0,    0,    1]
 
#Multiple thread support
multiple_thread: 1
 
#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 15            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 1              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
 
#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
 
#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.04          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.004         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.0004         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80766     # gravity magnitude
 
#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.0                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
 
#rolling shutter parameters
rolling_shutter: 0                  # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 
 
#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0

min_dist不能太大，否则会跑飞，这个参数控制的是提取角点进行光流追踪的密度，代表角点之间的最近距离，值越小表示提取角点数量越大。
cam0.yaml写法如下

%YAML:1.0
---
model_type: KANNALA_BRANDT
camera_name: camera
image_width: 512
image_height: 512
mirror_parameters:
   xi: 3.6313355285286337e+00
   gamma1: 2.1387619122017772e+03
projection_parameters:
   k2: 0.0034823894022493434
   k3: 0.0007150348452162257
   k4: -0.0020532361418706202
   k5: 0.00020293673591811182
   mu: 190.97847715128717
   mv: 190.9733070521226
   u0: 254.93170605935475
   v0: 256.8974428996504

5.3 evo评估

根据上面得到的两个文件，计算轨迹误差

evo_ape tum vio_loop.csv ~/TUM/dataset-room1_512_16/dso/gt_imu.tum  -va --plot --plot_mode xyz
evo_rpe tum vio_loop.csv ~/TUM/dataset-room1_512_16/dso/gt_imu.tum -r full -va --plot --plot_mode xyz