基于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);
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);
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()函数,如下修改
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
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
estimate_extrinsic: 0
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: 1
max_cnt: 150
min_dist: 15
freq: 10
F_threshold: 1.0
show_track: 1
equalize: 1
fisheye: 1
max_solver_time: 0.04
max_num_iterations: 8
keyframe_parallax: 10.0
acc_n: 0.04
gyr_n: 0.004
acc_w: 0.0004
gyr_w: 2.0e-5
g_norm: 9.80766
estimate_td: 0
td: 0.0
rolling_shutter: 0
rolling_shutter_tr: 0
load_previous_pose_graph: 0
pose_graph_save_path: "/home/tony-ws1/output/pose_graph/"
save_image: 1
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
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
