基于EVO的二次开发：读写轨迹文件

最新推荐文章于 2024-07-01 23:04:36 发布

寒墨阁

最新推荐文章于 2024-07-01 23:04:36 发布

阅读量1.3w

点赞数 5

分类专栏： python学习文章标签： python 自动驾驶经验分享 slam evo

本文链接：https://blog.csdn.net/hanmoge/article/details/111979469

版权

python学习专栏收录该内容

18 篇文章 0 订阅

订阅专栏

EVO是一套基于python的slam算法输出定位轨迹后处理工具：

github链接

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]这些索引的位姿数据。