trajectopy 0.1.7

Trajectory Evaluation in Python

Trajectopy - Trajectory Evaluation in Python

Welcome to Trajectopy, a Python package designed to simplify the alignment and comparison of trajectories using an intuitive graphical user interface (GUI).

Key Features

Trajectopy offers a range of features, including:

• Interactive GUI: A user-friendly interface that enables seamless interaction with your trajectory data, making it easy to visualize, align, and compare trajectories.
• 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.

Installation

Clone this repository using:

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

cd trajectopy

Install

python3 -m pip install -e .

Run

trajectopy

Importing Trajectories

Trajectories can be imported using the "Add" button below the trajectory table or by dragging files into the area of the trajectory table. 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"

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. The alignment settings can be found in the right-click trajectory settings.

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
• Time shift
• Lever arm x
• Lever arm y
• Lever arm z

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.

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.

Matching tolerances can be set in the settings of the corresponding trajectory.

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
2. Find corresponding pose pair in estimated trajectory
3. Compute relative translation and rotation between the reference pose pair
4. Compute relative translation and rotation between the estimated pose pair
5. Compute translational and rotational difference between 3) and 4)
6. Divide 5) by the distance or the time that separated both poses (e.g. 100 m).

Units are m/m: %, deg/m for distance based comparison and m/s, deg/s for time-based comparison. Pose distances can be set in the settings of the corresponding trajectory. Furthermore, the user can choose to either use consecutive pose pairs (non-overlapping) or all posible pairs (overlapping).

Spatial Sorting

When dealing with repeated trajectories, it may be useful to spatially sort the trajectory for further analysis. 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 MLS approximation is reconstructed using a minimum spanning tree. Depending on the algorithm, colinear points are discarded in this step.

Approximation

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

Sessions

Sessions allow you to export all imported and computed trajectories and results into a folder. This folder can be imported again after restarting trajectopy, saving time and keeping related data together.

Settings

Plot Settings

Setting	Description
Position Unit	Affects deviation plots only
Stair Histogram	If set to true, deviation histograms are not filled and only a stair is visible. (see also matplotlib documentation)
Smoothing Window Width	For better visibility in the deviation plots, the deviations are smoothed with a sliding window of a certain width. Smoothing can be turned off by setting a width of 0.0 or -1.0
Show Mean Line	Toggles the visibility of a red mean line in deviation plots.
Grid Megapixels	For every scatter plot, the trajectory data is divided into a grid with a certain number of pixels that can be set with this setting. This prevents slow plotting with large trajectories.
RMS Window Width	Similar to the Smoothing Window Width, this parameter sets the width of a sliding window used to smooth the RMS values that are mapped onto the trajectory in scatter plots. Smoothing can be turned off by setting a width of 0.0 or -1.0
Show Zero-Crossing in Colorbars	Each scatter plot of deviations includes a colorbar, which can be toggled to show the zero-crossing explicitly.
Colorbar Step Divisor, Clip Colorbar at X Sigma	The colorbar limits are determined by either the minimum or maximum values, or by the X-sigma range ("Clip-Colorbar at X Sigma" Setting) around the mean deviation, depending on which limit is more restrictive. Starting at the lower bound, the colorbar ticks are placed at positions each separated by the total colorbar range divided by the colorbar step divisor.
No axis	Toggles the visibility of axes in scatter plots
Rotate Scatter Plots to Main Axis	When set to true, the scatter plot trajectories will be rotated to align their main axis of extension (determined through principal component analysis) with the x-axis. This can be beneficial for elongated trajectories.

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