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.683007
      mean	12.773848
    median	8.709058
       min	7.277328
      rmse	15.745825
       sse	303467.550378
       std	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 --plot

APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)

       max	0.124530
      mean	0.045489
    median	0.037617
       min	0.009132
      rmse	0.053137
       sse	2.961871
       std	0.027464