Skip to content

Trajectory

class Trajectory

Class representing a trajectory, i.e. position and orientation of a plattform over time

  • Position-Computations are always done in a local frame
  • Time stamps are always in UTC time
  • Rotations are always defined in a East-North-Up frame

Attributes:

  • pos (PointSet): Position of the trajectory
  • rot (RotationSet): Orientation of the trajectory
  • tstamps (np.ndarray): Time stamps of the trajectory
  • name (str): Name of the trajectory
  • arc_lengths (np.ndarray): Arc lengths of the trajectory
  • speed_3d (np.ndarray): 3D speed of the trajectory
  • sorting (Sorting): Sorting of the trajectory

Methods:

  • __init__: Initialize trajectory
  • __str__: Returns string describing trajectory
  • __repr__: Returns string representation of trajectory
  • __len__: Return number of poses
  • __eq__: Check if two trajectories are equal
  • init_arc_lengths: Initialize arc lengths
  • copy: Deep copy of itself
  • from_file: Create trajectory from file
  • sort_switching_index: Returns the index that switches the sorting of the trajectory
  • sorting_index: Returns the index that sorts the trajectory
  • function_of: Returns the function of the trajectory
  • function_of_unit: Returns the unit of the function of the trajectory
  • function_of_label: Returns the label of the function of the trajectory
  • xyz: Returns the xyz coordinates of the trajectory
  • quat: Returns the quaternion of the trajectory
  • rpy: Returns the roll, pitch, yaw of the trajectory
  • to_dataframe: Returns a pandas dataframe containing tstamps, xyz, quat and speed_3d of the trajectory
  • to_file: Writes trajectory to ascii file
  • from_numpy: Initialize trajectory using numpy arrays
  • se3: Returns SE3 pose list
  • se3.setter: Sets position and rotation from se3 list
  • data_rate: Returns data rate
  • total_length: Return the total trajectory arc_length
  • speed_3d: Returns computed speeds or custom speeds
  • speed_3d.setter: Sets custom speeds
  • speed: Returns trajectory speeds calculated using consecutive point distances
  • crop: Crops trajectory to timespan defined by t_start and t_end
  • interpolate: Interpolates a trajectory to specified timestamps
  • _interpolate_rotations: Function for rotation interpolation of a trajectory
  • _interpolate_positions: Function for position interpolation of a trajectory
  • match_timestamps: Truncates trajectory to only those poses where the timestamps exactly match "tstamps"
  • intersect: Intersects trajectory with a given timestamp vector
  • apply_index: Applies index to the trajectory
  • apply_transformation: Applies transformation to trajectory

method Trajectory.__init__

__init__(
    pos: pointset.pointset.PointSet,
    rot: Optional[trajectopy.core.rotationset.RotationSet] = None,
    tstamps: Optional[numpy.ndarray] = None,
    name: str = '',
    arc_lengths: Optional[numpy.ndarray] = None,
    speed_3d: Optional[numpy.ndarray] = None,
    sorting: trajectopy.core.sorting.Sorting = <Sorting.TIME: 'time'>
) → None

property Trajectory.data_rate

Returns data rate

property Trajectory.function_of

Returns the function of the trajectory

property Trajectory.function_of_label

Returns the label of the function of the trajectory

property Trajectory.function_of_unit

Returns the unit of the function of the trajectory

property Trajectory.has_orientation

Returns True if orientation is available

property Trajectory.quat

Returns the quaternion of the trajectory

In contrast to the rot.as_quat() attribute, this method reflects the current sorting of the trajectory.

property Trajectory.rpy

Returns the roll, pitch, yaw of the trajectory

In contrast to the rot.as_euler(seq="xyz") attribute, this method reflects the current sorting of the trajectory.

property Trajectory.se3

Returns SE3 pose list

property Trajectory.sort_switching_index

Returns the index that switches the sorting of the trajectory

property Trajectory.sorting_index

Returns the index that sorts the trajectory

property Trajectory.speed

Returns trajectory speeds calculated using consecutive point distances

property Trajectory.speed_3d

Returns computed speeds or custom speeds

property Trajectory.total_length

Return the total trajectory arc_length.

property Trajectory.xyz

Returns the xyz coordinates of the trajectory

In contrast to the pos.xyz attribute, this method reflects the current sorting of the trajectory.

method Trajectory.adopt_first_orientation

adopt_first_orientation(
    trajectory: 'Trajectory',
    inplace: bool = True
) → Trajectory

Transform trajectory so that the first orientation is identical in both

Args:

  • trajectory (Trajectory): Target Trajectory
  • inplace (bool, optional): Perform in-place. Defaults to True.

Returns:

  • Trajectory: Transformed trajectory

method Trajectory.adopt_first_pose

adopt_first_pose(trajectory: 'Trajectory', inplace: bool = True) → Trajectory

Transform trajectory so that the first pose is identical in both

Args:

  • trajectory (Trajectory): Target Trajectory
  • inplace (bool, optional): Perform in-place. Defaults to True.

Returns:

  • Trajectory: Transformed trajectory

method Trajectory.adopt_first_position

adopt_first_position(
    trajectory: 'Trajectory',
    inplace: bool = True
) → Trajectory

Transform trajectory so that the first position is identical in both

Args:

  • trajectory (Trajectory): Target Trajectory
  • inplace (bool, optional): Perform in-place. Defaults to True.

Returns:

  • Trajectory: Transformed trajectory

method Trajectory.apply_alignment

apply_alignment(
    alignment_result: trajectopy.core.alignment.result.AlignmentResult,
    inplace: bool = True
) → Trajectory

Transforms trajectory using alignment parameters.

After computing the alignment parameters needed to align two trajectories, they can be applied to arbitrary trajectories.

Args: alignment_result (AlignmentResult) - inplace (bool, optional): Perform in-place. Defaults to True.

Returns:

  • Trajectory: Aligned trajectory

method Trajectory.apply_index

apply_index(
    index: Union[list, numpy.ndarray],
    inplace: bool = True
) → Trajectory

Applies index to the trajectory

This will be done either in-place or using a new instance of a trajectory. The index can be used to filter and / or sort the components of the trajectory.

Those components are: - timestamps (tstamps) - positions (xyz) - rotations (rot) - arc lengths (arc_lengths) - sorting index (_sort_index)

Args:

  • index (Union[list, np.ndarray]): index that should be applied
  • inplace (bool, optional): Perform in-place. Defaults to True.

Returns:

  • Trajectory: Trajectory with index applied.

method Trajectory.apply_transformation

apply_transformation(
    transformation: numpy.ndarray,
    inplace: bool = True
) → Trajectory

Applies transformation to trajectory

Args:

  • transformation (np.ndarray): 4x4 Transformation matrix
  • inplace (bool, optional): Perform in-place. Defaults to True.

Returns:

  • Trajectory: Transformed trajectory

method Trajectory.approximate

approximate(
    approximation_settings: trajectopy.core.settings.approximation.ApproximationSettings = ApproximationSettings(fe_int_size=0.15, fe_min_obs=25, rot_approx_win_size=0.15),
    inplace: bool = True
) → Trajectory

Approximates the trajectory using piecewise cubic polynomial.

Args:

  • approximation_settings (ApproximationSettings): Approximation settings.

Returns:

  • Trajectory: Approximated trajectory.

method Trajectory.copy

copy() → Trajectory

Deep copy of itself

method Trajectory.crop

crop(
    t_start: float,
    t_end: float,
    inverse: bool = False,
    inplace: bool = True
) → Trajectory

Crops trajectory to timespan defined by t_start and t_end

Args:

  • t_start (float): Start timestamp of desired time span
  • t_end (float): End timestamp of desired time span
  • inverse (bool, optional): If true, 'crop' turns into 'cut', i.e. everthing outside of t_start and t_end will be removed. Defaults to False.
  • inplace (bool, optional): Perform crop in-place. Defaults to True.

Returns:

  • Trajectory: Cropped trajectory

classmethod Trajectory.from_file

from_file(filename: str, io_stream: bool = False) → Trajectory

Create trajectory from file

The file must be a csv file containing columns for at least the timestamp, x, y and z coordinates of the trajectory. Those fields must be named "t", "px", "py" and "pz" in the header using the #fields tag. However, by default a trajectory with "t,px,py,pz,qx,qy,qz,qw" fields is assumed. Additional fields include the arc length, specified by "l", and the speed, specified by "vx", "vy" and "vz". The delimiter can be specified using the #delimiter tag. The default delimiter is a comma.

Args:

  • filename (str): path to file
  • io_stream (bool, optional): If true, the file is read from a stream.

Returns:

  • Trajectory: trajectory object

classmethod Trajectory.from_numpy

from_numpy(
    xyz: numpy.ndarray,
    quat: numpy.ndarray,
    tstamps: numpy.ndarray,
    epsg: int = 0
) → Trajectory

Initialize trajectory using numpy arrays

method Trajectory.init_arc_lengths

init_arc_lengths()

method Trajectory.interpolate

interpolate(
    tstamps: Union[list, numpy.ndarray],
    inplace: bool = True
) → Trajectory

Interpolates a trajectory to specified timestamps

This method removes timestamps from tstamps if they lie outside of the timestamp range of the trajectory (self). Since providing values for those timestamps would require an extrapolation and not an interpolation, this behaviour is consistent with the definition of this method.

Args:

  • tstamps (list): Interpolation timestamps
  • inplace (bool, optional): Perform in-place interpolation. Defaults to True.

Returns:

  • Trajectory: Interpolated trajectory

method Trajectory.intersect

intersect(
    tstamps: numpy.ndarray,
    max_gap_size: float = 2.0,
    inplace: bool = True
) → Trajectory

Intersects trajectory with a given timestamp vector

After intersection, the trajectory covers the same timespan as 'tstamps'. Further, gaps larger than 'max_gap_size' are removed. If two consecutive timespans in tstamps have a difference of more than 'max_gap_size' seconds, they are considered as the limits of a gap. All timestamps of the trajectory that lie within this gap will be removed.

Args:

  • tstamps (np.ndarray): Intersection timespans
  • max_gap_size (float, optional): Maximum allowed gap between timespans. If Defaults to 0.5.
  • inplace (bool, optional): Perform intersection in-place. Defaults to True.

Raises:

  • ValueError: If timespans do not overlap.

Returns:

  • Trajectory: Intersected trajectory

method Trajectory.match_timestamps

match_timestamps(tstamps: numpy.ndarray, inplace: bool = True) → Trajectory

Truncates trajectory to only those poses where the timestamps exactly match "tstamps"

Args:

  • tstamps (np.ndarray): Input timestamps
  • inplace (bool, optional): Perform matching in-place. Defaults to True.

Returns:

  • Trajectory: Trajectory with matched timestamps

method Trajectory.sort_spatially

sort_spatially(
    sorting_settings: trajectopy.core.settings.sorting.SortingSettings = SortingSettings(discard_missing=True, voxel_size=0.05, movement_threshold=0.005, k_nearest=4),
    inplace: bool = True
) → Trajectory

Sorts the trajectory spatially.

Args:

  • sorting_settings (SortingSettings): Sorting settings.
  • inplace (bool, optional): Whether to sort the trajectory in-place. Defaults to True.

Returns:

  • Trajectory: Sorted trajectory.

method Trajectory.to_dataframe

to_dataframe(sort_by: str = '') → DataFrame

Returns a pandas dataframe containing tstamps, xyz, quat and speed_3d of the trajectory.

The dataframe is sorted by the current sorting attribute (time or arc_length).

Args:

  • sort_by (str, optional): Column to sort by. This overrides the current sort_by attribute.

Returns:

  • pd.DataFrame: Trajectory as dataframe

method Trajectory.to_file

to_file(filename: str, mode: str = 'w') → None

Writes trajectory to ascii file

The first line will always be the epsg information. After that, the trajectory data is written.

Args:

  • filename (str): Output filename