trajectopy-core 1.3.1

Trajectory Evaluation in Python

Trajectopy - Trajectory Evaluation in Python

Trajectopy is a toolbox for empirical trajectory evaluation.

This is the core implementation of Trajectopy without any PyQt dependencies. For advanced functionality and an intuitive graphical user interface, see https://github.com/gereon-t/trajectopy.

Key Features

Trajectopy offers a range of features, including:

• Alignment Algorithm: An advanced algorithm that can be tailored to the specific application and supports a similarity transformation, a leverarm and a time shift estimation.
• Comparison Metrics: Absolute and relative comparison metrics that can be computed using various pose-matching methods
• Data Import/Export: Support for importing and exporting data, ensuring compatibility with your existing workflows.
• Customizable Visualization: A flexible visualization that allows users to customize plot styles, tailoring the output to their specific needs.

Table of Contents

• Installation
• Exemplary Evaluation
• Importing Trajectories
• Aligning Trajectories
• Comparing Trajectories
• Spatial Sorting
• Approximation
• Plotting

Installation

Since version 1.1.0, trajectopy supports Python 3.8.2

Create virtual environment (Optional but recommended)

Unix

python3 -m venv .venv

Windows

python -m venv .venv

Activate (Unix)

source .venv/bin/activate

or (Windows)

.\.venv\Scripts\activate

Installation via pip

Update pip:

pip install --upgrade pip

Install trajectopy:

pip install trajectopy-core

Or using the repository:

git clone https://github.com/gereon-t/trajectopy-core.git

cd trajectopy-core

poetry install

Exemplary Evaluation

This section shows how to use trajectopy to evaluate two trajectories. The example data can be found in the example_data folder. The full code can be found in the example_scripts folder.

Absolute Trajectory Error (ATE)

from trajectopy_core.evaluation.comparison import compare_trajectories_absolute
from trajectopy_core.alignment.actions import align_trajectories
from trajectopy_core.settings.alignment_settings import AlignmentSettings
from trajectopy_core.settings.matching_settings import MatchingMethod, MatchingSettings
from trajectopy_core.trajectory import Trajectory

gt_traj = Trajectory.from_file("./example_data/KITTI_gt.traj")
est_traj = Trajectory.from_file("./example_data/KITTI_ORB.traj")


# align
alignment_settings = AlignmentSettings()  # Default settings
alignment = align_trajectories(
    traj_from=est_traj,
    traj_to=gt_traj,
    alignment_settings=alignment_settings,
    matching_settings=MatchingSettings(method=MatchingMethod.NEAREST_TEMPORAL),
)
est_traj_aligned = est_traj.apply_alignment(alignment)

ate_result = compare_trajectories_absolute(traj_ref=gt_traj, traj_test=est_traj_aligned)

Plotted ATE result:

Relative Pose Error (RPE)

from trajectopy_core.evaluation.comparison import compare_trajectories_relative
from trajectopy_core.settings.comparison_settings import RelativeComparisonSettings
from trajectopy_core.trajectory import Trajectory

gt_traj = Trajectory.from_file("./example_data/KITTI_gt.traj")
est_traj = Trajectory.from_file("./example_data/KITTI_ORB.traj")

# compute RPE
settings = RelativeComparisonSettings()  # Default settings
rpe_result = compare_trajectories_relative(traj_ref=gt_traj, traj_test=est_traj, settings=settings)

Plotted RPE result:

Trajectory Plotting

from matplotlib import pyplot as plt
from trajectopy_core.plotting.trajectory_plot import plot_trajectories
from trajectopy_core.trajectory import Trajectory

gt_traj = Trajectory.from_file("./example_data/KITTI_gt.traj")
est_traj = Trajectory.from_file("./example_data/KITTI_ORB.traj")

plot_trajectories([gt_traj, est_traj], dim=2)
plt.show()

Importing Trajectories

Trajectory files must be ASCII files with a csv-like layout, by default, trajectopy filters for the ".traj" extension. The default column structure that can be read without any configuration is the following:

time	position x	position y	position z	quaternion x	quaternion y	quaternion z	quaternion w

Columns are expected to be separated by commas by default.

It is recommended to provide a header at the beginning of the trajectory file. Header entries always begin with a "#". Below you can find a table of all allowed header entries and their meaning.

Header	Description
#name	The name provided here is displayed in the table view and in plots of the trajectory
#epsg	EPSG Code of the datum of the input positions. Required, if geodetic datum transformations are desired. Default: 0, meaning local coordinates without any known geodetic datum
#fields	Describes the columns of the ASCII trajectory file. Separated with commas. field name Meaning t time l arc lengths in meters px position x / lat (degrees only) py position y / lon (degrees only) pz position z qx quaternion x qy quaternion y qz quaternion z qw quaternion w ex euler angle x ey euler angle y ez euler angle z vx speed x vy speed y vz speed z Example: "#fields t,px,py,pz" Note: The only column that is allowed to appear multiple times is the "t" column.
#delimiter	Delimiter used to separate the columns within the file. Default: ","
#nframe	Definition of the navigation-frame the orientations of the trajectory refer to. Choices: "enu": East North Up or "ned": North East Down. Default: "enu"
#rot_unit	Unit of the orientations. Choices: "deg": Degree, "rad": Radians. Default: "rad"
#time_format	Format of the timestamps / dates. Choices: "unix": Unix timestamps (since 01-01-1970), "datetime": Human readable date-times. Default: "unix"
#time_offset	Offset in seconds that is applied to the imported timestamps. Default: 0.0
#datetime_format	Format of the datetimes. Only relevant if "time_format" is "datetime". Default: "%Y-%m-%d %H:%M:%S.%f"
#datetime_timezone	Time zone of the timestamps. During import, all timestamps are converted to UTC considering the input time zone. Choices: Time zone or "GPS"
#sorting	Sorting of the input data. Choices: "chrono": Chronologically sorted data (usually the case), "spatial": Spatially sorted data, i.e. along the arc length. Default: "chrono"
#state	States describing what processing steps the data already has passed. States: "approximated", "interpolated", "intersected", "aligned", "matched", "sorting_known"

New since 0.9.2: Experimental ROS bag support for geometry_msgs/msg/PoseStamped messages. Files must have a ".bag" extension. Poses must have positions and orientations. One file can contain multiple trajectories published under different topics.

Aligning Trajectories

The trajectories to be compared could be defined in different coordinate systems and/or refer to different positions on the same platform due to different mounting positions. There may also be a small synchronization offset. To solve this problem, trajectopy provides functionality to align two trajectories using least squares adjustment theory. This allows estimation of up to 11 parameters, all of which can be turned on or off individually to tailor the alignment to the sensor modalities.

Alignment Settings

Setting	Description
Minimum Speed	If set to a value larger than 0.0, only poses with a speed higher than the specified value are used for alignment. This is only relevant for the time shift estimation where higher speed usually lead to better results.
Time Start, Time End	Time range given in seconds relative to the start of the trajectory that should be aligned. If set to non-zero values, only poses in the specified time span are used for alignment.
Error Probability	Used for stochastic tests. During the alignment, the functional relationship and the stochastic model are tested for consistency. Besides a global test, there are also tests for each observation group. Observation groups are: xy_from, z_from, xy_to, z_to, roll_pitch, yaw, speed.
Observation Standard Deviations	Standard deviations of the trajectory data involved.
Estimation Of	Toggle for individual parameters that should be estimated. Furthermore, the user can specify which components of the 3d speed vector should be used for time shift estimation.

Parameters that can be estimated:

• Similarity x translation
• Similarity y translation
• Similarity z translation
• Similarity x rotation
• Similarity y rotation
• Similarity z rotation
• Similarity scale
• (small) Time shift
• Lever arm x
• Lever arm y
• Lever arm z
• Sensor rotation (experimental): Use this, if both sensors are mounted on the platform with different orientations

Both trajectories should be roughly synchronized to improve trajectory matching before alignment.

In addition to least squares alignment, you can also perform a basic trajectory alignment by aligning only the first pose.

Comparing Trajectories

This toolbox allows the comparison of two trajectories using absolute and relative metrics. Before comparison, both trajectories to be compared must be matched. After this, either absolute or relative metrics can be computed. If both trajectory are defined in different coordinate systems, it is recommended to align them accordingly.

Matching

There are three different types of trajectory matching implemented in trajectopy:

• Matching via interpolation: Poses are matched by interpolating one trajectory onto the timestamps of the other.
• Matching via Nearest Temporal Neighbor: Poses are matched using their timestamps while considering some pre-defined tolerance.
• Matching via Nearest Spatial Neighbor: Poses are matched using their nearest neighbors while considering some pre-defined distance tolerance.

Absolute

This metric is often referred to as Absolute Trajectory Error (ATE). It measures the translational and rotational difference between two matched poses. By default, trajectopy splits the deviations into vertical cross-track, horizontal cross-track, and along-track deviations to simplify interpretation. This behavior can be turned off.

Relative

For this metric, relative pose-pair differences are compared. The distance between two poses can be specified by the user and can be either time- or distance-based. The comparison involves finding pose pairs separated by a specific distance or time interval, computing the relative translation and rotation between the reference and estimated pose pairs, and calculating the translational and rotational difference normalized by the distance or time that separated the poses.

1. Find pose pair separated by e.g. 100 m in reference trajectory. This pair represents the start and end poses of a sequence of size $N$.

2. Find the corresponding pose pair in estimated trajectory

3. Compute relative translation and rotation between the reference pose pair

   $\Delta_{P~ref} = P_{ref, 1}^{-1} \cdot P_{ref, N}$

4. Compute relative translation and rotation between the estimated pose pair

   $\Delta_{P~est} = P_{est, 1}^{-1} \cdot P_{est, N}$

5. Compute transformation between 3) and 4)

6. Compute translation and rotation error from 5)

7. Divide 6) by the distance or the time that separated both poses (e.g. 100 m).

This metric does not require both trajectories to be aligned. Units are m/m: %, deg/m for distance based comparison and m/s, deg/s for time-based comparison.

Example:

• Minimum pose distance: 100
• Maximum pose distance: 800
• Distance step: 100
• Distance unit: Meter

Results in pose distances: [100 m, 200 m, 300 m, 400 m, 500 m, 600 m, 700 m, 800 m]

Furthermore, the user can choose to either use consecutive pose pairs (non-overlapping) or all posible pairs (overlapping).

Comparison Settings

Setting	Description
Maximum Time Difference (Temporal Matching) [s]	Maximum allowed time difference when matching two trajectories using their timestamps
Maximum Distance (Spatial Matching) [m]	During spatial matching, positions are matched using a nearest neighbor approach. This parameter sets the maximum allowed distance between matched positions.
Minimum pose distance	Minimum pose pair distance to be considered during RPE computation. Default: 100
Maximum pose distance	Maximum pose pair distance to be considered during RPE computation. Default: 800
Distance step	Step in which the pose distance increases. Default: 100
Distance unit	Unit of the pose distance. Either meter or second. Default: meter
Use all pairs	If toggled, overlapping pairs will be used for relative metrics

Spatial Sorting (Experimental)

When dealing with repeated trajectories, it may be useful to spatially sort the trajectory for further analysis (Tombrink et al. 2023). This reconstructs the point order as a function of the trajectory length. Trajectopy uses a combination of an iterative voxel-based moving least-squares approximation and a minimum spanning tree to solve this problem.

The point cloud is divided into voxels of a given size. Then, for each point, a number of neighboring voxels are requested. A 3D line is estimated using the points within these voxels. Finally, the current point is projected onto the line to obtain the approximated point. The spatial sorting of this Moving-Least-Squares (MLS) approximation is reconstructed using a minimum spanning tree. Depending on the algorithm, colinear points are discarded in this step.

Sorting Settings

Setting	Description
Voxel size [m]	Size of the voxel grid used for moving-least-squares approximation
k-nearest	Number of voxels used for MLS
Movement threshold [m]	MLS will be performed iteratively until the points move less than this specified threshold.

Approximation

The trajectory can be approximated using piecewise cubic polynomials for the positions and sliding window based smoothing for the rotations. All observations are assumed to be of equal uncertainty and uncorrelated. The window sizes and the minimum number of observations that should be within an interval can be set by the user.

Approximation Settings

Setting	Description
Window size (Positions) [m]	The x,y and z- position data is split up into intervals of a size set by this parameter. In each interval, a cubic polynomial approximates the data as best as possible while ensuring C1 continuity.
Minimum observation per interval	Minimum number of observations that should be in one interval. Intervals are merged to meet this requirement
Rotation Approximation Technique	Either sliding window based approach or (experimental) lap interpolation (only possible for spatially sorted trajectories)
Windows size (Rotations) [m]	Sliding window size used for computing the moving-average rotations

Plotting

Custom Matplotlib Style

You can use a custom plotting style by placing a custom.mplstyle file in the directory you launched trajectopy in. For instructions on how to define custom plotting styles, please see https://matplotlib.org/stable/tutorials/introductory/customizing.html. Below you can see the default style of trajectopy

figure.figsize: 8, 6
figure.facecolor: white
font.size: 14
font.family: serif
axes.prop_cycle: cycler("color", ["1E88E5", "#FFC107", "#004D40", "#D81B60", "#2bd2bb", "#a3bbf1", "#3c41fd", "#cc5510", "#3b0732", "#88122b", "#bccb70", "dc9c54"])
axes.facecolor: E2E2E2
axes.edgecolor: white
axes.grid: True
grid.color: white
grid.linewidth: 0.3
axes.grid.which: major
axes.axisbelow: True
legend.facecolor: white
lines.linestyle: -
lines.marker: .
savefig.dpi: 1500
savefig.format: pdf