evo评测VINS-MONO---TUM数据集

为了想测试室外场景下VINS-MONO定位情况，室外环境的KITTY数据集因无时间戳，EVO测评较困难。
故选择TUM数据集，outdoors1~8(户外)：在校园内的室外场景拍摄，但是数据包里只在开始和结束有真实值，没办法与真值全程评测。（outdoors4)
所以再选择个室内 room1~6(室内)：对应有5个图像序列，整个过程都带有运动捕捉系统提高的真实值进行测试。（room4)

修改VINS-mono轨迹保存代码

直接输出的结果格式并不能用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输出结果的格式更改完成，接下去修改真实值的格式。

修改config文件

想要得到带有回环检测的结果需要修改tum_config.yaml文件，首先修改输出路径，有两个地方可以改成同一个路径，第6行的output_path修改为自己指定的路径，然后第70行pose_graph_save_path修改为指定路径，然后开启回环检测，第67行loop_closure改为1，这样才会输出带回环检测的结果。

真实值修改为tum格式

真实值为对应的.tar文件中的gt_imu.csv文件，由于EVO并不支持这种格式，需要先进行格式转换，转换成.tum文件再和VINS运行的结果进行比较。
转换的命令为：

evo_traj euroc gt_imu.csv --save_as_tum

得到的gt_imu.tum就是可以使用EVO和VINS结果进行比较的格式。

终端运行

roscore
roslaunch vins_estimator tum.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play dataset-outdoors4_512_16.bag

EVO评测

evo_traj tum vins_result_loop_tumout.txt -p --plot_mode=xyz
--------------------------------------------------------------------------------
name:   vins_result_loop_tumout
infos:  6092 poses, 992.077m path length, 696.322s duration

evo_ape tum gt_imu.tum vins_result_loop_tumout.txt -va --plot
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)max   86.683007mean   12.773848median 8.709058min 7.277328rmse    15.745825sse    303467.550378std    9.206509

因outdoors1~8(户外)：在校园内的室外场景拍摄，只在开始和结束有真实值，不是全程有真值，所以无法进行APE绝对位姿误差的评断。

因room1~6(室内)：对应有5个图像序列，整个过程都带有运动捕捉系统提高的真实值。故找段室内场景进行测试。

roscore
roslaunch vins_estimator tum.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play dataset-room4_512_16.bag

evo_ape tum gt_imu.tum vins_result_loop_tumroom.txt -va --plotAPE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)max   0.124530mean    0.045489median  0.037617min 0.009132rmse    0.053137sse 2.961871std 0.027464