EVO是一套基于python的slam算法输出定位轨迹后处理工具:
EVO被广泛用来对slam算法输出的定位轨迹进行与真值轨迹的对比、作图等后处理,具体应用以上链接有介绍,网上也已经有足够多的中文文章介绍EVO的使用。
本文主要介绍EVO的一个核心文件trajectory.py,以及file_interface.py提供的一系列工具函数,基于这些工具,我们可以很方便地对TUM、KITTI、rosbag等格式的定位轨迹做读写。
1. 文件读写函数:
py文件路径:evo/evo/tools/file_interface.py,打开这个文件就可以看到很多read/write函数,例如:
def read_tum_trajectory_file(file_path) -> PoseTrajectory3D:
"""
读tum格式文件,输入文件路径即可;输出是EVO的内部PoseTrajectory3D,稍后会介绍
parses trajectory file in TUM format (timestamp tx ty tz qx qy qz qw)
:param file_path: the trajectory file path (or file handle)
:return: trajectory.PoseTrajectory3D object
"""
def write_tum_trajectory_file(file_path, traj: PoseTrajectory3D,
confirm_overwrite: bool = False) -> None:
"""
写tum格式文件,输入要写的文件路径,含有要写的轨迹内容的PoseTrajectory3D对象,和一个是否覆盖已有文件的布尔量
:param file_path: desired text file for trajectory (string or handle)
:param traj: trajectory.PoseTrajectory3D
:param confirm_overwrite: whether to require user interaction
to overwrite existing files
"""
def read_kitti_poses_file(file_path) -> PosePath3D:
"""
读KITTI格式文件
parses pose file in KITTI format (first 3 rows of SE(3) matrix per line)
:param file_path: the trajectory file path (or file handle)
:return: trajectory.PosePath3D
"""
def write_kitti_poses_file(file_path, traj: PosePath3D,
confirm_overwrite: bool = False) -> None:
"""
写KITTI格式文件
:param file_path: desired text file for trajectory (string or handle)
:param traj: trajectory.PosePath3D or trajectory.PoseTrajectory3D
:param confirm_overwrite: whether to require user interaction
to overwrite existing files
"""
def read_euroc_csv_trajectory(file_path) -> PoseTrajectory3D:
"""
读EuRoC格式csv文件
parses ground truth trajectory from EuRoC MAV state estimate .csv
:param file_path: <sequence>/mav0/state_groundtruth_estimate0/data.csv
:return: trajectory.PoseTrajectory3D object
"""
def read_bag_trajectory(bag_handle, topic: str) -> PoseTrajectory3D:
"""
读rosbag文件,此函数的输入除了一个指定的rosbag的句柄以外(相当于指定打开的文件路径),还需要指定要读的rostopic名称
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: trajectory topic of supported message type,
or a TF trajectory ID (e.g.: '/tf:map.base_link' )
:return: trajectory.PoseTrajectory3D
"""
def write_bag_trajectory(bag_handle, traj: PoseTrajectory3D, topic_name: str,
frame_id: str = "") -> None:
"""
写rosbag文件,此函数的输入为一个rosbag的句柄,要写的轨迹内容的PoseTrajectory3D对象,rostopic名称和该rostopic的frame_id
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param traj: trajectory.PoseTrajectory3D
:param topic_name: the desired topic name for the trajectory
:param frame_id: optional ROS frame_id
"""
总结起来就是,file_interface.py中的读文件函数将会读取轨迹文件并保存为一个PoseTrajectory3D类的实例,而写文件函数将会把PoseTrajectory3D实例写为指定路径上指定格式的文件。
2. PoseTrajectory3D类:
py文件路径:evo/evo/core/trajectory.py
PoseTrajectory3D和PosePath3D这两个EVO的核心类就在这个文件中。所有的定位轨迹在EVO内部都会被储存为PoseTrajectory3D类的实例,再进行计算误差、作图等操作。
PosePath3D是PoseTrajectory3D的基类,具体的类的实现可以详读代码evo/evo/core/trajectory.py。PoseTrajectory3D具有很多好的方法,方便我们进行二次开发,例如:
- PosePath3D类里有:
def positions_xyz(self) -> np.ndarray: 以np.ndarray类型返回轨迹上所有的xyz空间坐标;
def distances(self) -> np.ndarray: 返回一个数组,是增量式的轨迹路程;
def orientations_quat_wxyz(self) -> np.ndarray: 返回所有姿态的四元数,wxyz形式;
def get_orientations_euler(self, axes=“sxyz”) -> np.ndarray: 返回所有姿态的欧拉角,可以指定坐标轴顺序;
def poses_se3(self) -> typing.Sequence[np.ndarray]: 返回所有姿态的se3旋转矩阵;
def num_poses(self) -> int: 返回轨迹中的位姿数据个数;
def path_length(self) -> float: 返回轨迹总路程;
def transform(self, t: np.ndarray, right_mul: bool = False,
propagate: bool = False) -> None: 对轨迹整体进行一个矩阵t的左乘或右乘。
def scale(self, s: float) -> None: 对轨迹进行尺度放大或缩小
def align(self, traj_ref: ‘PosePath3D’, correct_scale: bool = False,
correct_only_scale: bool = False,
n: int = -1) -> geometry.UmeyamaResult: 将轨迹与traj_ref尽量对齐,使用Umeyama算法,对两点集之间的变换参数进行最小二乘估计,返回用于对齐的旋转矩阵、位移矩阵和尺度
def align_origin(self, traj_ref: ‘PosePath3D’) -> np.ndarray: 将轨迹原点与traj_ref原点对齐,返回位移矩阵
以上几个方法:transform/scale/align/align_origin与evo进行轨迹误差评价时的可选参数–align --correct_scale有关,具体可见:EVO待评价轨迹几何对齐
def reduce_to_ids(self, ids: typing.Sequence[int]) -> None: 根据函数输入ids,即位姿索引序列裁剪轨迹数据,只保留ids指定的位姿。该方法主要用于根据时间戳对不同轨迹做同步
- PoseTrajectory3D里有:
def reduce_to_ids(self, ids: typing.Sequence[int]) -> None: 继承自PosePath3D类,功能相同
def get_infos(self) -> dict: 继承自PosePath3D类,返回轨迹的基本信息如位姿个数、起止位姿、轨迹总路程、时长和起止时间
def get_statistics(self) -> dict: 继承自PosePath3D类,返回轨迹的速度信息如最大最小速度、平均速度等
PoseTrajectory3D实例的初始化也很简单:
def __init__(
self, positions_xyz: typing.Optional[np.ndarray] = None,
orientations_quat_wxyz: typing.Optional[np.ndarray] = None,
timestamps: typing.Optional[np.ndarray] = None,
poses_se3: typing.Optional[typing.Sequence[np.ndarray]] = None,
meta: typing.Optional[dict] = None):
"""
:param timestamps: optional nx1 list of timestamps
"""
super(PoseTrajectory3D,
self).__init__(positions_xyz, orientations_quat_wxyz, poses_se3,
meta)
# this is a bit ugly...
if timestamps is None:
raise TrajectoryException("no timestamps provided")
self.timestamps = np.array(timestamps)
只要把一条轨迹的时间戳、位姿xyz坐标、位姿的姿态(四元数或旋转矩阵形式)准备好,填入即可;再进一步应用第一节中的读写函数可以输出成各种想要的格式。
3. 示例:
简单使用示例如下:
打开一个python命令行窗口,我这里是在ubuntu 16.04下打开的python 3.5.2的窗口(EVO也是对应的版本,更高版本也类似),首先添加EVO路径:
>>> import sys
>>> import os
>>> sys.path.insert(0,os.path.abspath('你的EVO安装或者下载保存路径/evo/'))
import导入file_interface:
>>> from evo.tools import file_interface
用tum读文件函数读取一个示例文件:
>>> traj = file_interface.read_tum_trajectory_file('文件路径/evo_test.txt')
输出得到的“traj”即为PoseTrajectory3D类型的数据:
>>> type(traj)
<class 'evo.core.trajectory.PoseTrajectory3D'>
以下是traj的一些方法返回的结果:
>>> traj.get_infos()
{'nr. of poses': 60, 'path length (m)': 0.47392244174816445, 't_start (s)': 1311868164.363181, 'pos_start (m)': array([0., 0., 0.]), 'pos_end (m)': array([ 0.30755463, 0.11409403, -0.18693894]), 't_end (s)': 1311868166.331189, 'duration (s)': 1.9680078029632568}
>>> traj.get_statistics()
{'v_min (m/s)': 0.06434166470965388, 'v_max (km/h)': 1.4786647502810522, 'v_min (km/h)': 0.23162999295475398, 'v_avg (km/h)': 0.8718316807948172, 'v_avg (m/s)': 0.24217546688744923, 'v_max (m/s)': 0.41074020841140335}
>>> traj.positions_xyz
array([[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[-5.01930000e-04, 1.01386000e-03, -2.00978600e-03],
[ 4.29820000e-04, 1.96032600e-03, -4.89852200e-03],
[ 5.79095100e-03, 4.64138100e-03, -7.30242800e-03],
[ 7.05512500e-03, 6.19986800e-03, -9.46804700e-03],
[ 1.00280290e-02, 4.42431000e-03, -1.33460390e-02],
[ 1.06258000e-02, 3.76662100e-03, -1.62867420e-02],
[ 1.38213720e-02, 4.54199900e-03, -1.93799210e-02],
[ 1.52620660e-02, 5.50546800e-03, -2.21289810e-02],
[ 1.73042570e-02, 4.91991200e-03, -2.60796230e-02],
[ 2.46654280e-02, 6.93547800e-03, -2.85710750e-02],
[ 2.91411300e-02, 3.68245100e-03, -3.22448720e-02],
[ 3.55532910e-02, 3.02793200e-03, -3.50451250e-02],
[ 4.19072660e-02, 3.56638100e-03, -3.67779730e-02],
[ 4.90812620e-02, 3.09080000e-03, -3.86409720e-02],
[ 5.76151800e-02, 1.09549800e-03, -4.29062660e-02],
[ 6.42168000e-02, 2.22150900e-03, -4.35798910e-02],
[ 7.28496980e-02, 1.24074600e-03, -4.66098820e-02],
......
[ 3.07554632e-01, 1.14094026e-01, -1.86938941e-01]])
可以用reduce_to_ids修改traj:
>>> traj.reduce_to_ids([0,1,2,3,4,5,6,7,8,9])
>>> traj.positions_xyz
array([[ 0. , 0. , 0. ],
[-0.00050193, 0.00101386, -0.00200979],
[ 0.00042982, 0.00196033, -0.00489852],
[ 0.00579095, 0.00464138, -0.00730243],
[ 0.00705512, 0.00619987, -0.00946805],
[ 0.01002803, 0.00442431, -0.01334604],
[ 0.0106258 , 0.00376662, -0.01628674],
[ 0.01382137, 0.004542 , -0.01937992],
[ 0.01526207, 0.00550547, -0.02212898],
[ 0.01730426, 0.00491991, -0.02607962]])
可以看出,只保留了[0,1,2,3,4,5,6,7,8,9]这些索引的位姿数据。