Matching

matching

match_non_overlapping_stamps

match_non_overlapping_stamps(
    trajectory: Trajectory,
    other: Trajectory,
    max_distance: float = 0.0,
) -> float

Roughly matches two trajectories.

Parameters:

• trajectory (Trajectory) –

  Trajectory to match.

• other (Trajectory) –

  Other trajectory to match against.

• max_distance (float, default: 0.0 ) –

  Maximum distance for spatial matching. Defaults to 0.0.

Returns:

• float ( float ) –

  Mean offset.

Source code in trajectopy\processing\matching.py

def match_non_overlapping_stamps(trajectory: Trajectory, other: Trajectory, max_distance: float = 0.0) -> float:
    """Roughly matches two trajectories.

    Args:
        trajectory (Trajectory): Trajectory to match.
        other (Trajectory): Other trajectory to match against.
        max_distance (float, optional): Maximum distance for spatial matching. Defaults to 0.0.

    Returns:
        float: Mean offset.
    """
    other, trajectory = _match_trajectories_spatial(
        trajectory=other.copy(), other=trajectory.copy(), max_distance=max_distance
    )
    mean_offset = np.median(trajectory.index - other.index)
    logger.info("Median index offset: %.3f", mean_offset)
    return mean_offset

match_stamps

match_stamps(
    trajectory: Trajectory,
    index: ndarray,
    inplace: bool = True,
) -> Trajectory

Truncates trajectory to only those poses where the indices exactly match "index".

Parameters:

• trajectory (Trajectory) –

  Input trajectory.

• index (ndarray) –

  Input indices.

• inplace (bool, default: True ) –

  Perform matching in-place. Defaults to True.

Returns:

• Trajectory ( Trajectory ) –

  Trajectory with matched indices.

Source code in trajectopy\processing\matching.py

def match_stamps(trajectory: Trajectory, index: np.ndarray, inplace: bool = True) -> Trajectory:
    """Truncates trajectory to only those poses where the indices exactly match "index".

    Args:
        trajectory (Trajectory): Input trajectory.
        index (np.ndarray): Input indices.
        inplace (bool, optional): Perform matching in-place. Defaults to True.

    Returns:
        Trajectory: Trajectory with matched indices.
    """
    traj_self = trajectory if inplace else trajectory.copy()
    _, idx_self, _ = np.intersect1d(traj_self.index, index, return_indices=True)
    traj_self.mask(idx_self)
    return traj_self

match_trajectories

match_trajectories(
    trajectory: Trajectory,
    other: Trajectory,
    matching_settings: MatchingSettings | None = None,
    inplace: bool = True,
) -> tuple[Trajectory, Trajectory]

Matches two trajectories using the specified method.

Supported methods:

• INTERPOLATION: Interpolates the test trajectory onto the reference trajectory using its timestamps. The interpolation is linear for both positions and rotations (SLERP).
• NEAREST_TEMPORAL: Finds the nearest temporal match without interpolation by finding the nearest timestamp in the target trajectory for each timestamp in the source trajectory.
• NEAREST_SPATIAL: Finds the nearest spatial match without interpolation by finding the nearest pose in the target trajectory for each pose in the source trajectory using Euclidean distance.
• NEAREST_SPATIAL_INTERPOLATED: Finds the nearest k spatial matches and spatially interpolates using a 3d line. Both trajectories will have the length of the test trajectory. This method does not support rotation matching.

Parameters:

• trajectory (Trajectory) –

  Trajectory to match.

• other (Trajectory) –

  Other trajectory to match against.

• matching_settings (MatchingSettings, default: None ) –

  Matching settings. Defaults to MatchingSettings().

• inplace (bool, default: True ) –

  Whether to modify the input trajectories. Defaults to True.

Returns:

• tuple[Trajectory, Trajectory] –

  Tuple[Trajectory, Trajectory]: Matched trajectories.

Source code in trajectopy\processing\matching.py

def match_trajectories(
    trajectory: Trajectory,
    other: Trajectory,
    matching_settings: settings.MatchingSettings | None = None,
    inplace: bool = True,
) -> tuple[Trajectory, Trajectory]:
    """Matches two trajectories using the specified method.

    Supported methods:

    - **INTERPOLATION**: Interpolates the test trajectory onto the reference trajectory using
      its timestamps. The interpolation is linear for both positions and rotations (SLERP).
    - **NEAREST_TEMPORAL**: Finds the nearest temporal match without interpolation by finding
      the nearest timestamp in the target trajectory for each timestamp in the source trajectory.
    - **NEAREST_SPATIAL**: Finds the nearest spatial match without interpolation by finding
      the nearest pose in the target trajectory for each pose in the source trajectory using
      Euclidean distance.
    - **NEAREST_SPATIAL_INTERPOLATED**: Finds the nearest k spatial matches and spatially
      interpolates using a 3d line. Both trajectories will have the length of the test trajectory.
      This method does not support rotation matching.

    Args:
        trajectory (Trajectory): Trajectory to match.
        other (Trajectory): Other trajectory to match against.
        matching_settings (MatchingSettings, optional): Matching settings. Defaults to
            MatchingSettings().
        inplace (bool, optional): Whether to modify the input trajectories. Defaults to True.

    Returns:
        Tuple[Trajectory, Trajectory]: Matched trajectories.
    """
    if matching_settings is None:
        matching_settings = settings.MatchingSettings()

    trajectory = trajectory if inplace else trajectory.copy()
    other = other if inplace else other.copy()

    if (
        trajectory.sorting == Sorting.PATH_LENGTH
        and other.sorting == Sorting.PATH_LENGTH
        and matching_settings.method
        in [settings.MatchingMethod.NEAREST_TEMPORAL, settings.MatchingMethod.INTERPOLATION]
        and not np.array_equal(trajectory.index, other.index)
    ):
        _adopt_reference_path_lengths(trajectory=trajectory, other=other)

    if not trajectory.overlaps_with(other) and matching_settings.method in [
        settings.MatchingMethod.NEAREST_TEMPORAL,
        settings.MatchingMethod.INTERPOLATION,
    ]:
        logger.warning("Trajectories do not overlap! Performing rough matching first.")
        timeshift = match_non_overlapping_stamps(
            trajectory=other, other=trajectory, max_distance=matching_settings.max_distance
        )
        logger.info("Rough matching index offset: %.3f", timeshift)

        if trajectory.sorting == Sorting.TIME:
            trajectory.timestamps = trajectory.timestamps + timeshift
        else:
            trajectory.path_lengths = trajectory.path_lengths + timeshift

    logger.info("Matching trajectories using method %s", matching_settings.method.name)

    if matching_settings.method == settings.MatchingMethod.INTERPOLATION:
        return _match_trajectories_interpolation(
            trajectory=trajectory, other=other, max_gap_size=matching_settings.max_gap_size
        )

    if matching_settings.method == settings.MatchingMethod.NEAREST_TEMPORAL:
        return _match_trajectories_temporal(
            trajectory=trajectory, other=other, max_distance=matching_settings.max_time_diff
        )

    if matching_settings.method == settings.MatchingMethod.NEAREST_SPATIAL:
        return _match_trajectories_spatial(
            trajectory=trajectory, other=other, max_distance=matching_settings.max_distance
        )

    if matching_settings.method == settings.MatchingMethod.NEAREST_SPATIAL_INTERPOLATED:
        return _match_trajectories_spatial_interpolation(
            trajectory=trajectory,
            other=other,
            max_distance=matching_settings.max_distance,
            k_nearest=matching_settings.k_nearest,
        )

    raise ValueError(f"Matching method {matching_settings.method} not supported!")