使用evo工具评测SLAM

evo是一款用于视觉里程计和slam问题的轨迹评估工具。核心功能是能够绘制相机的轨迹,或评估估计轨迹与真值的误差。支持多种数据集的轨迹格式(TUM、KITTI、EuRoC MAV、ROS的bag),同时支持这些数据格式之间进行相互转换。在此仅对其基本功能做简要介绍。并且介绍如何修改经典的SLAM算法以输出可使用evo评测的轨迹。

github地址

安装

  1. 使用pypi直接安装:
install evo --upgrade --no-binary evo ```
1
2
3
4

2. 本地编译安装

``` pip install --editable . --upgrade --no-binary evo

安装完毕后,在命令行输入evo,若显示了相关信息,则表明安装成功。若提示”command not found”也不用惊慌,很多人遇到这种问题,重启电脑即可找到evo相应指令。

使用

指标

  • evo_ape - absolute pose error - 绝对误差计算绝对位姿误差(absolute pose error),用于整体评估整条轨迹的全局一致性;
    evo_rpe:计算相对位姿误差(relative pose error),用于评价轨迹局部的准确性。
  • evo_rpe - relative pose error - 相对误差 (相对误差=绝对误差/真值)

工具命令

  • evo_traj - tool for analyzing, plotting or exporting one or more trajectories 对轨迹进行分析、画图

  • evo_res - tool for comparing one or multiple result files from evo_ape or evo_rpe 对比多个结果

  • evo_fig - (experimental) tool for re-opening serialized plots (saved with --serialize_plot)

  • evo_config - tool for global settings and config file manipulation -设置参数

  • -va a 对齐轨迹

evo绘制轨迹的指令为:evo_traj,后跟必要参数有:数据的格式(tum/kitti/bag/euroc等),轨迹文件。轨迹文件可以有多个,例如:
evo_traj tum traj1.txt traj2.txt
这个指令只是显示轨迹的基本信息,若要绘制轨迹,则增加可选参数 -p 或 –plot
evo_traj tum traj1.txt –p

说明

  • 对比/绘制VINS-mono/fusion的轨迹时需要对其代码进行修改,具体参考该博客.

使用例子

EuRoC

画groundtruth的轨迹

1
evo_traj euroc ./groundtruth/MH_01_data.csv -p --plot_mode=xyz

画某一次结果的轨迹

1
evo_traj tum ./result/MH_01/loop_result.csv -p --plot_mode=xyz

TUM

groundtruth.txt 为外部运动捕捉系统采集到的相机位姿,格式为(time, t x , t y , t z , q x , q y , q z , q w ),

测试过程

VINS-Mono

VINS-mono的估计结果需要按照TUM格式输出 因此要对代码做一些调整。在一下两个文件中修改即可。

  • “vins_result_loop” : defined in [path to Vins folder]/pose_graph/src/pose_graph.cpp ; line 156 or 630. The format is timestamp + position(x,y, z) + quaternion(qw, qx, qy, qz).

  • “vins_result_no_loop”: defined in [path to Vins folder]/vins_estimator/src/utility/visualization.cpp in function pubOdometry().

    The format is timestamp + position(x,y, z) + quaternion(qw, qx, qy, qz) + velocity(x,y,z).

代码修改过程:

visualization.cpp -> Pubodometry ()

1
2
3
4
5
6
7
8
9
10
11
12
// write result to file 为了按照TUM格式输出 调换了位置
double turetime = header.stamp.toSec();
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC << turetime <<" "
<< 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;

*pose_graph.cpp *

updatePath()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
   if (SAVE_LOOP_PATH)
{
ofstream loop_path_file("/home/guoben/Documents/output/loop_result.csv", ios::app);
double turetime = cur_kf->time_stamp;
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file << turetime << " "
<< P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << 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 << (*it)->time_stamp << " ";
loop_path_file << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
loop_path_file.close();
}

画某一个结果的轨迹

1
evo_traj tum ./loop_result.csv -p --plot_mode=xyz

对比结果

1
evo_rpe euroc ./groundtruth/MH_01_data.csv ./result/MH_01/vins_result_loop.csv -va -r full --plot

MSCKF

1
2
3
4
5
6
7
8
9
10
// ofstream foutC("/home/guoben/Documents/output/result_vio.csv", ios::app);
// foutC.setf(ios::fixed, ios::floatfield);
// foutC << odom_msg.header.stamp.toSec() << " "
// << odom_msg.pose.pose.position.x << " "
// << odom_msg.pose.pose.position.y << " "
// << odom_msg.pose.pose.position.z << " "
// << odom_msg.pose.pose.orientation.x << " "
// << odom_msg.pose.pose.orientation.y << " "
// << odom_msg.pose.pose.orientation.z << " "
// << odom_msg.pose.pose.orientation.w << endl;
  • © 2019-2022 guoben
  • PV: UV:

微信