使用evo评测ekf_pose和ndt_pose

ATE定义

参考link

安装evo

由于ros1使用python2.7，如果需要使用evo里将rosbag转为tum格式的命令，需要安装evo1.12.0。因为更高版本的evo就不支持python2.7了。注意pip必须保证pip2。

git clone https://github.com/MichaelGrupp/evo.git -b v1.12.0
cd evo
pip2 install --editable . --upgrade --no-binary evo
python setup.py install

测试安装成功

evo_ape -h
cd test/data
evo_traj kitti KITTI_00_ORB.txt KITTI_00_SPTAM.txt --ref=KITTI_00_gt.txt -p --plot_mode=xz
evo_ape kitti KITTI_00_gt.txt KITTI_00_ORB.txt -va --plot --plot_mode xz
evo_rpe tum fr2_desk_groundtrutr2_desk_ORB.txt -va --plot --plot_mode xyz

ros录制轨迹包

rosbag record  -O  bag_name.bag /ekf_pose /ndt_pose 

查看rosbag 类型

rosbag    info   bag_name.bag

rosbag 分割

rosbag filter input.bag output.bag "(topic == '/velodyne_point_cloud' or topic =='/visensor/imu' or topic == '/visensor/left/image_raw') and (t.to_sec() >= 1506117983.884751 and t.to_sec() <= 1506118069.884751)"

使用evo把rosbag转tum

evo_traj bag bag_name.bag /ekf_pose --save_as_tum
evo_traj bag bag_name.bag /ndt_pose --save_as_tum

两种轨迹可视化

evo_traj tum ndt_pose.tum --ref=ekf_pose.tum -p --plot_mode=xy -v --sync --save_plot traj_map.pdf

RPE指标（相对轨迹误差）

evo_rpe tum ekf_pose.tum ndt_pose.tum  -v -p --plot_mode=xy

APE指标（绝对轨迹误差）

制作groundtruth的tum：相关代码在此仓库中

git clone https://gitee.com/wangchunzheng/tum.git

代码说明：

首先把待操作的tum文件放入main.py同级文件夹内
然后修改main.py中 “# 设置groundtruth的位姿值”注释下面代码
euler欧拉角格式：euler(roll, pitch, yaw)，编号从0～2
trans位置坐标格式：trans(x, y, z)，编号从0～2

最后生成ekf_pose_gt.tum和ndt_pose_gt.tum。下面进行evo评估。
位置结果

evo_ape tum ekf_pose_gt.tum ekf_pose.tum -v -p --plot_mode=xy --save_plot ekf_APE_trans.pdf -r trans_part
evo_ape tum ndt_pose_gt.tum ndt_pose.tum -v -p --plot_mode=xy --save_plot ndt_APE_trans.pdf -r trans_part

姿态结果

evo_ape tum ekf_pose_gt.tum ekf_pose.tum -v -p --plot_mode=xy --save_plot ekf_APE_rot.pdf -r angle_rad
evo_ape tum ndt_pose_gt.tum ndt_pose.tum -v -p --plot_mode=xy --save_plot ndt_APE_rot.pdf -r angle_rad

在evo设置输出误差项对应的option为-r/–pose_relation，并有如下模式：
full, trans_part, angle_rad, angle_deg, rot_part
相比较而言，用平移误差单位为m，旋转误差单位可以是度或弧度，更容易理解。

注意，如果是雷达与相机两坐标系比较，由于刚性连接，两者原点存在固定旋转矩阵，需要转换；如果是ndt_pose与ekf_pose两坐标系比较，原点相同，采样频率不同倒问题不大，但采样时间不同，比方说在当前时间戳前后0.01秒之内的，可以认为是对应的位姿。另外，如果想消除z轴的影响，可以将高度数据设置为0。

evo_traj tum -h 帮助

optional arguments:
  -h, --help            show this help message and exit
  -f, --full_check      run all checks and print all stats

algorithm options:
  -a, --align           alignment with Umeyama's method (no scale) - requires --ref
  -s, --correct_scale   scale correction with Umeyama's method - requires --ref
  --n_to_align N_TO_ALIGN
                        the number of poses to use for Umeyama alignment, counted from the start (default: all)
  --align_origin        align the trajectory origin to the origin of the reference trajectory
  --sync                associate trajectories via matching timestamps - requires --ref
  --transform_left TRANSFORM_LEFT
                        path to a .json file with a transformation to apply to  the trajectories (left multiplicative)
  --transform_right TRANSFORM_RIGHT
                        path to a .json file with a transformation to apply to the trajectories (right_multiplicative)
  --propagate_transform
                        with --transform_right: transform each pose and  propagate resulting drift to the next.
  --invert_transform    invert the transformation of the .json file
  --ref REF             trajectory that will be marked/used as the reference
  --t_offset T_OFFSET   add a constant timestamp offset (not adding to --ref  trajectory)
  --t_max_diff T_MAX_DIFF
                        maximum timestamp difference for data association
  --merge               merge the trajectories in a single trajectory

output options:
  -p, --plot            show plot window
  --plot_relative_time  show timestamps relative to the start of the reference
  --plot_mode {xy,xz,yx,yz,zx,zy,xyz}
                        the axes for plot projection
  --ros_map_yaml ROS_MAP_YAML
                        yaml file of an ROS 2D map image (.pgm/.png) that will be drawn into the plot
  --save_plot SAVE_PLOT
                        path to save plot
  --save_table SAVE_TABLE
                        path to save table with statistics
  --serialize_plot SERIALIZE_PLOT
                        path to serialize plot (experimental)
  --save_as_tum         save trajectories in TUM format (as *.tum)
  --save_as_kitti       save poses in KITTI format (as *.kitti)
  --save_as_bag         save trajectories in ROS bag as <date>.bag
  --logfile LOGFILE     Local logfile path.

usability options:
  --no_warnings         no warnings requiring user confirmation
  -v, --verbose         verbose output
  --silent              don't print any output
  --debug               verbose output with additional debug info
  -c CONFIG, --config CONFIG
                        .json file with parameters (priority over command line args)

翻译

可选参数：
-h	显示此帮助消息并退出
-f		检查运行所有检查并打印所有统计数据

算法选项：
-a	Umeyama方法对齐（无刻度）-需要 --ref
-s	Umeyama方法纠正刻度 -需要 --ref
--n_to_align N_TO_ALIGN	用于Umeyama对齐的位姿数，从开始算起（默认值：全部）
--align_origin	将轨迹原点与参考轨迹的原点对齐
--sync 	匹配时间戳同步关联轨迹 -需要--ref
--transform_left TRANSFORM_LEFT	json文件的路径，带有要应用于轨迹的旋转矩阵（左乘法）
--transform_right TRANSFORM_RIGHT		json文件的路径，带有要应用于轨迹的旋转矩阵（右乘法）
--propagate_transform	使用--transform_right：变换每个姿势，并将产生的漂移传播到下一个姿势。
--invert_transform		json文件里旋转矩阵的逆
--ref REF		将被标记/用作参考的轨迹
--t_offset T_OFFSET		添加一个恒定的时间戳偏移量（不添加到--ref轨迹）
--t_max_diff T_MAX_DIFF		数据关联的最大时间戳差异
--merge		将轨迹合并为一条轨迹

输出选项：
-p		显示绘图窗口
--plot_relative_time		显示相对于引用开始的时间戳
--plot_mode {xy,xz,yx,yz,zx,zy,xyz}		用于绘图投影的轴
--ros_map_yaml ROS_MAP_YAML		ROS 2D地图图像（.pgm/.png）的yaml文件，将绘制到绘图中
--save_plot SAVE_PLOT		保存绘图的路径
--save_table SAVE_TABLE		使用统计信息保存表的路径
--serialize_plot SERIALIZE_PLOT		序列化绘图的路径（实验）
--save_as_tum		以tum格式保存traj（另存为*.tum）
--save_as_kitti		以kitti格式保存pose（如*.kitti）
--save_as_bag		以rosbag格式保存轨迹（如<date>.bag）
--logfile LOGFILE		本地日志文件路径。

可用性选项：
--no_warnings		无需用户确认的警告
-v		详细输出
--silent		不打印任何输出
--debug		带有额外调试信息的详细输出
-c CONFIG, --config CONFIG		.json文件和参数（优先级高于命令行参数）

evo_ape tum -h 帮助内容

Absolute pose error (APE) metric app for TUM trajectory files - (c) evo
authors

positional arguments:位置参数：
  ref_file              reference trajectory file 在前
  est_file              estimated trajectory file 在后

optional arguments:可选参数：
  -h, --help            show this help message and exit
  --t_max_diff T_MAX_DIFF            maximum timestamp difference for data association  数据关联的最大时间戳差异
  --t_offset T_OFFSET    constant timestamp offset for data association 数据关联的常数时间戳偏移量

algorithm options:
  -r {full,trans_part,rot_part,angle_deg,angle_rad}, --pose_relation {full,trans_part,rot_part,angle_deg,angle_rad} {平移误差（单位米）、旋转误差（矩阵二范数）、（单位度）、（单位弧度）}
                        pose relation on which the APE is based APE评估的位姿参数
  -a, --align           alignment with Umeyama's method (no scale)
  -s, --correct_scale   correct scale with Umeyama's method
  --n_to_align N_TO_ALIGN
                        the number of poses to use for Umeyama alignment, counted from the start (default: all)
  --align_origin        align the trajectory origin to the origin of the
                        reference trajectory

output options:
  -p, --plot            show plot window
  --plot_mode {xy,xz,yx,yz,zx,zy,xyz}
                        the axes for plot projection
  --plot_colormap_max PLOT_COLORMAP_MAX
                        the upper bound used for the color map plot (default:
                        maximum error value)
  --plot_colormap_min PLOT_COLORMAP_MIN
                        the lower bound used for the color map plot (default:
                        minimum error value) 用于颜色贴图打印的下限（默认值：最小错误值）
  --plot_colormap_max_percentile PLOT_COLORMAP_MAX_PERCENTILE
                        percentile of the error distribution to be used as the
                        upper bound of the color map plot (in %, overrides
                        --plot_colormap_max) 用作颜色映射图上限的误差分布的百分位数（单位：%）
  --plot_full_ref       plot the full, unsynchronized reference trajectory 绘制完整的、不同步的参考轨迹
  --ros_map_yaml ROS_MAP_YAML
                        yaml file of an ROS 2D map image (.pgm/.png) that will
                        be drawn into the plot 
  --save_plot SAVE_PLOT
                        path to save plot
  --serialize_plot SERIALIZE_PLOT
                        path to serialize plot (experimental)
  --save_results SAVE_RESULTS
                        .zip file path to store results
  --logfile LOGFILE     Local logfile path.

usability options:
  --no_warnings         no warnings requiring user confirmation
  -v, --verbose         verbose output
  --silent              don't print any output
  --debug               verbose output with additional debug info
  -c CONFIG, --config CONFIG
                        .json file with parameters (priority over command line
                        args)