vins fusion测评工具——evo

以Euroc双目+IMU为例

前提条件

• 能够成功运行vins fusion

EVO安装与使用

可参考链接:
链接: 测评工具evo安装与使用

打开终端安装EVO

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

修改Euroc配置文件euroc_stereo_imu_config.yaml

• 输出路径修改

output_path: "/home/jia/Desktop/algorithm/VINS-Fusion1/evo_result/"

pose_graph_save_path: "/home/jia/Desktop/algorithm/VINS-Fusion1/evo_result/" # save and load path

修改源代码

• vins fusion保存的数据集格式evo无法直接利用，需要改成evo能够使用的数据格式

visualization.cpp中pubOdometry()函数

• 修改前

        // write result to file
        ofstream foutC(VINS_RESULT_PATH, ios::app);//这意味着如果文件已经存在，新的数据将追加到文件末尾。如果文件不存在，将创建一个新文件
        foutC.setf(ios::fixed, ios::floatfield);//这行代码将浮点数输出格式设置为固定格式，这意味着浮点数将以固定的小数位数显示
        foutC.precision(0);//这行代码将浮点数的精度设置为0，这意味着它们将被显示为没有小数位数的整数。
        foutC << header.stamp.toSec() * 1e9 << ",";//时间戳+；
        foutC.precision(5);//输出小数点为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();//这行代码在写入数据后关闭文件
        Eigen::Vector3d tmp_T = estimator.Ps[WINDOW_SIZE];//这行代码创建一个名为 tmp_T 的新的 Eigen::Vector3d 对象，并将其赋值为 estimator.Ps[WINDOW_SIZE] 的值。
        printf("time: %f, t: %f %f %f q: %f %f %f %f \n", header.stamp.toSec(), tmp_T.x(), tmp_T.y(), tmp_T.z(),
                                                          tmp_Q.w(), tmp_Q.x(), tmp_Q.y(), tmp_Q.z());

• 修改后

          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;
        foutC.close();//这行代码在写入数据后关闭文件
        Eigen::Vector3d tmp_T = estimator.Ps[WINDOW_SIZE];//这行代码创建一个名为 tmp_T 的新的 Eigen::Vector3d 对象，并将其赋值为 estimator.Ps[WINDOW_SIZE] 的值。
        printf("time: %f, t: %f %f %f q: %f %f %f %f \n", header.stamp.toSec(), tmp_T.x(), tmp_T.y(), tmp_T.z(),
                                                          tmp_Q.w(), tmp_Q.x(), tmp_Q.y(), tmp_Q.z());

pose_graph.cpp中的updatePath()函数

• 修改前

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

• 修改后

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

pose_graph.cpp文件中addKeyFrame()函数

• 修改前

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

• 修改后

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

pose_graph_node.cpp中的main()函数

• 修改前

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vio_loop.csv";

• 修改后

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vio_loop.txt";

• 重新编译vins fusion(记得把之前编译的build和devel文件删除）

EVO绘制轨迹

• 重新运行vins fusion,运行完成后会看到在自己设置的目录下生成的文件
• 回环检测

evo_traj tum vio_loop.txt -p

• 非回环检测

evo_traj tum vio.csv -p

• 运行上面命令即可看到轨迹
  

真值的获取

Euroc官网

• 下载解压后在mav0/state_groundtruth_estimate0可以看到三个文件，其中data.csv就是真值文件，输入命令保存为tum格式

evo_traj euroc data.csv --save_as_tum

• 可用命令进行对比

evo_ape tum vins.csv data.tum -va -p

• 考虑平移部分得到的ape，单位为m

evo_ape tum data.tum vins_loop.csv -r trans_part -va --plot --plot_mode xyz

• 考虑旋转角得到的ape,单位弧度（rad）

evo_ape tum data.tum vins_loop.csv -r angle_rad -va --plot --plot_mode xyz

• 考虑旋转+平移

evo_ape tum data.tum vins_loop.csv -r full -va --plot --plot_mode xyz