EVO评估Vins-Mono

安装EVO

略。

修改Vins-Mono代码

tum数据集格式介绍

注意：旋转由四元数表示，虚部在前，实部在后。

时间戳	平移x	平移y	平移z	旋转x	旋转y	旋转z	旋转w
timestamp	tx	ty	tz	Qx	Qy	Qz	Qw

修改VINS-mono轨迹保存代码

修改6个地方

序号	文件	函数
1	vins_estimator/src/utility/visualization.cpp	pubOdometry
2	pose_graph/src/pose_graph.cpp	updatePath
3	pose_graph/src/pose_graph.cpp	addKeyFrame
4	vins_estimator/src/parameters.cpp	readParameters
5	pose_graph/src/pose_graph_node.cpp	main
6	config/euroc/euroc_config.yaml	output_path

（1）修改 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;

修改为：

 // write result to file
	 ofstream foutC(VINS_RESULT_PATH, ios::app);
	 foutC.setf(ios::fixed, ios::floatfield);
	 foutC.precision(9);
	 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;
	       //<< estimator.Vs[WINDOW_SIZE].x() << ","
	       //<< estimator.Vs[WINDOW_SIZE].y() << ","
	       //<< estimator.Vs[WINDOW_SIZE].z() << "," << endl;
	 foutC.close();

（2）修改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(0);
            loop_path_file << (*it)->time_stamp * 1e9 << ",";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << ","
                  << P.y() << ","
                  << P.z() << ","
                  << Q.w() << ","
                  << Q.x() << ","
                  << Q.y() << ","
                  << Q.z() << ","
                  << endl;
            loop_path_file.close();
        }

修改为：

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();
        }

（3）修改pose_graph.cpp文件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(0);
        loop_path_file << cur_kf->time_stamp * 1e9 << ",";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << ","
              << P.y() << ","
              << P.z() << ","
              << Q.w() << ","
              << Q.x() << ","
              << Q.y() << ","
              << Q.z() << ","
              << endl;
        loop_path_file.close();
 }

修改为：

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();
 }

（4）修改parameters.cpp文件readParameters函数

原来：

VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv";

修改为：

VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.tum";

（5）修改pose_graph_node.cpp文件main函数

原来：

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv";

修改为：

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.tum";

（6） 修改yaml文件

自己的结果输出路径

output_path: "/home/project/GVinsmono_ws/output/"

运行脚本

序号	文件	功能
1	run_estimator.sh	打开Vins-Mono
2	run_rviz.sh	打开rviz可视化结果
3	run_rosbag.sh	播包
4	run_evo_loop.sh	评估带回环的轨迹结果
5	run_evo_noloop.sh	评估不带回环的轨迹结果

（1）run_estimator.sh

cd /home/project/GVinsmono_ws
source ./devel/setup.bash
roslaunch vins_estimator euroc.launch

（2）run_rviz.sh

cd /home/project/GVinsmono_ws
source ./devel/setup.bash
roslaunch vins_estimator vins_rviz.launch

（3）run_rosbag.sh

cd /home/datasets/euroc
rosbag play MH_01_easy.bag 

（4）run_evo_loop

cd /home/project/GVinsmono_ws/output
evo_ape tum data.tum vins_result_loop.tum -va --plot --plot_mode xyz

（5）run_evo_noloop.sh

cd /home/project/GVinsmono_ws/output
evo_ape tum data.tum vins_result_no_loop.tum -va --plot --plot_mode xyz